mechanic_integrators.cc

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// openCARP is an open cardiac electrophysiology simulator.
//
// Copyright (C) 2020 openCARP project
//
// This program is licensed under the openCARP Academic Public License (APL)
// v1.0: You can use and redistribute it and/or modify it in non-commercial
// academic environments under the terms of APL as published by the openCARP
// project v1.0, or (at your option) any later version. Commercial use requires
// a commercial license (info@opencarp.org).
//
// This program is distributed without any warranty; see the openCARP APL for
// more details.
//
// You should have received a copy of the openCARP APL along with this program
// and can find it online: http://www.opencarp.org/license
// ----------------------------------------------------------------------------
 
#include "mechanic_integrators.h"
#include <string.h>
 
namespace opencarp {
 
inline int get_preferred_int_order(SF::elem_t & type)
{
  // for testing purposes we might want to select the integration order per element type,
  // but in general we want second order integration for linear Ansatzfunctions
#if 0
  switch(type) {
    case SF::Tetra:
    case SF::Tri:
    case SF::Quad:
    case SF::Hexa:
      return 2;
 
    default:
    case SF::Prism:
    case SF::Pyramid:
      return 1;
  }
#else
  return 2;
#endif
}
 
 
 
 
inline void print_points(SF::Point* pts, int npts)
{
  for(int i=0; i<npts; i++)
    printf(" ( %g %g %g ) \n", pts[i].x, pts[i].y, pts[i].z);
 
  printf("\n");
}
 
inline void calc_deformation_gradient(const SF::element_view<mesh_int_t,mesh_real_t> & elem, int ipt, double * F, double * iF, double & detF, int index, double eps)
{
  SF::elem_t   type = elem.type();
  mesh_int_t nnodes = elem.num_nodes();
  int nint;
  SF::Point lpts[EP_MAX_LPOINTS];
  SF::Point updated_pts[EP_MAX_LPOINTS];
  SF::Point fib = elem.fiber();
  SF::Point ipts[EP_MAX_IPOINTS];
  double w[EP_MAX_LPOINTS];
  SF::dmat<double> shape;
  SF::dmat<double> gshape;
  double J[9];
  double detJ;
  const int int_order = get_preferred_int_order(type) - 1;
    
  SF::get_transformed_pts(elem, lpts, fib);
  SF::get_transformed_upd_pts(elem, updated_pts, fib);
  mesh_int_t ndof = nnodes;
  shape.set_size(4, ndof);
  gshape.set_size(4, ndof);
    
//  Change one variable with epsilon for the calculation of finite differences
  if(index > -1)
  {
    int pid = index/3;
    int direction = index%3;
    if(direction == 0)
    {
        updated_pts[pid].x += eps;
    }
    else if(direction == 1)
    {
        updated_pts[pid].y += eps;
    }
    else if(direction == 2)
    {
        updated_pts[pid].z += eps;
    }
    else
    {
        log_msg(0, 0, 0, "Point Update doesn't work");
    }
  }
    
  elem.integration_points(int_order, ipts, w, nint);
  SF::reference_shape(type, ipts[ipt], shape);
  SF::jacobian_matrix(shape, nnodes, lpts, J);
 
  SF::invert_jacobian_matrix(type, J, detJ);
 
  SF::shape_deriv(J, shape, ndof, gshape);
    
  SF::jacobian_matrix(gshape, nnodes, updated_pts, F); //calculate the deformation gradient at the integration point
    
  if (type == SF::Tri || type == SF::Line || type == SF::Quad) {
      F[8] = 1; //deformation gradient needs to be 3X3 and is Identity for no deformation
  }
  // copy deformation gradient for calculation of inverse
  for(size_t k = 0; k < 9; k++){
      iF[k] = F[k];
  }
  
  SF::invert_3x3(iF, detF);
}
 
 
inline void apply_active_tension(double * deformation_gradient, double * Piola_Kirchhoff_1_stress)
{
  timer_manager & tm = *user_globals::tm_manager;
  double time = tm.time + param_globals::dt; //Test
//  double S_act_ff = 60.0 * time / param_globals::tend;
  double S_act_ff;
  switch (param_globals::Ta.type) {
    case 0:
      S_act_ff = param_globals::Ta.strength * time / param_globals::tend;
      break;
    case 1:
      printf("Tanh model not yet implemented");
      break;
    case 2:
      printf("double hill model not yet implemented");
      break;
    case 3:
      printf("External active tension model input not yet implemented");
      break;
    default:
      printf("Please choose an existing active tension model");
      break;
  }
  double K_ff = 1.0;
 
  Piola_Kirchhoff_1_stress[0] += deformation_gradient[0] * S_act_ff * K_ff;
  Piola_Kirchhoff_1_stress[1] += deformation_gradient[1] * S_act_ff * K_ff;
  Piola_Kirchhoff_1_stress[2] += deformation_gradient[2] * S_act_ff * K_ff;
}
 
 
 
void mech_internal_forces_integrator::operator() (const SF::element_view<mesh_int_t, mesh_real_t> & elem, double * buff)
{
  mesh_int_t dpn_ele;
  dpn(dpn_ele);
  zero_buff(buff, SF_MAX_ELEM_NODES*dpn_ele);
  const size_t eidx = elem.element_index();
  SF::elem_t   type = elem.type();
  mesh_int_t   nnodes = elem.num_nodes();
  const int    int_order = get_preferred_int_order(type) - 1;
    
  const int    reg  = material.regionIDs.size() ? material.regionIDs[eidx] : 0;
  const RegionSpecs & reg_spec = material.regions[reg];
 
  assert(reg_spec.material->material_type == MechMat);
  mechMaterial & mat = *static_cast<mechMaterial*>(reg_spec.material);
  Basic_constitutive_model * constitutive_model;
  constitutive_model = constitutive_model_setup(mat.mat_t, param_globals::mech_mat_region[reg].model_params);
  
  SF::Point fib = elem.fiber(), she = elem.sheet();
  bool has_sheet = elem.has_sheet();
  if (!has_sheet)
  {
    SF::Point helper_dir;
    if(fib.x * fib.x < 0.9)
    {
      helper_dir = {1, 0, 0};
    }
    else
    {
      helper_dir = {0, 1, 0};
    }
    she = cross(fib, helper_dir);
    
  }
  SF::Point nor = cross(fib, she);
  normalize(fib);
  normalize(she);
  normalize(nor);
  SF::get_transformed_pts(elem, lpts, fib); //get point coordinates in reference shape
  #ifdef DEBUG_INT
    print_points(lpts, nnodes);
  #endif
 
 
  int ndof = nnodes;
  shape.set_size(4, ndof), gshape.set_size(4, ndof);
 
  int nint = 0;
  elem.integration_points(int_order, ipts, w, nint);
 
  double vol = 0.0;
  // Integration loop
  for(int iidx=0; iidx < nint; iidx++)
  {
    double detJ;
    double detF;
    SF::reference_shape(type, ipts[iidx], shape);
    SF::jacobian_matrix(shape, nnodes, lpts, J);
    SF::invert_jacobian_matrix(type, J, detJ);
    SF::shape_deriv(J, shape, ndof, gshape);
        
    calc_deformation_gradient(elem, iidx, F, iF, detF);
    
    // rotate deformation gradient into f, s, n coordinate system ///Write function to do that later
    double QFQT[9], QiFQT[9]; //rotated deformation gradient and the inverse
    QFQT[0] = fib.x * (fib.x * F[0] + fib.y * F[3] + fib.z * F[6]) + fib.y * (fib.x * F[1] + fib.y * F[4] + fib.z * F[7]) + fib.z * (fib.x * F[2] + fib.y * F[5] + fib.z * F[8]); //F_ff
    QFQT[1] = she.x * (fib.x * F[0] + fib.y * F[3] + fib.z * F[6]) + she.y * (fib.x * F[1] + fib.y * F[4] + fib.z * F[7]) + she.z * (fib.x * F[2] + fib.y * F[5] + fib.z * F[8]); //F_sf
    QFQT[2] = nor.x * (fib.x * F[0] + fib.y * F[3] + fib.z * F[6]) + nor.y * (fib.x * F[1] + fib.y * F[4] + fib.z * F[7]) + nor.z * (fib.x * F[2] + fib.y * F[5] + fib.z * F[8]); //F_nf
    QFQT[3] = fib.x * (she.x * F[0] + she.y * F[3] + she.z * F[6]) + fib.y * (she.x * F[1] + she.y * F[4] + she.z * F[7]) + fib.z * (she.x * F[2] + she.y * F[5] + she.z * F[8]); //F_fs
    QFQT[4] = she.x * (she.x * F[0] + she.y * F[3] + she.z * F[6]) + she.y * (she.x * F[1] + she.y * F[4] + she.z * F[7]) + she.z * (she.x * F[2] + she.y * F[5] + she.z * F[8]); //F_ss
    QFQT[5] = nor.x * (she.x * F[0] + she.y * F[3] + she.z * F[6]) + nor.y * (she.x * F[1] + she.y * F[4] + she.z * F[7]) + nor.z * (she.x * F[2] + she.y * F[5] + she.z * F[8]); //F_ns
    QFQT[6] = fib.x * (nor.x * F[0] + nor.y * F[3] + nor.z * F[6]) + fib.y * (nor.x * F[1] + nor.y * F[4] + nor.z * F[7]) + fib.z * (nor.x * F[2] + nor.y * F[5] + nor.z * F[8]); //F_fn
    QFQT[7] = she.x * (nor.x * F[0] + nor.y * F[3] + nor.z * F[6]) + she.y * (nor.x * F[1] + nor.y * F[4] + nor.z * F[7]) + she.z * (nor.x * F[2] + nor.y * F[5] + nor.z * F[8]); //F_sn
    QFQT[8] = nor.x * (nor.x * F[0] + nor.y * F[3] + nor.z * F[6]) + nor.y * (nor.x * F[1] + nor.y * F[4] + nor.z * F[7]) + nor.z * (nor.x * F[2] + nor.y * F[5] + nor.z * F[8]); //F_nn
    
    QiFQT[0] = fib.x * (fib.x * iF[0] + fib.y * iF[3] + fib.z * iF[6]) + fib.y * (fib.x * iF[1] + fib.y * iF[4] + fib.z * iF[7]) + fib.z * (fib.x * iF[2] + fib.y * iF[5] + fib.z * iF[8]); //iF_ff
    QiFQT[1] = she.x * (fib.x * iF[0] + fib.y * iF[3] + fib.z * iF[6]) + she.y * (fib.x * iF[1] + fib.y * iF[4] + fib.z * iF[7]) + she.z * (fib.x * iF[2] + fib.y * iF[5] + fib.z * iF[8]); //iF_sf
    QiFQT[2] = nor.x * (fib.x * iF[0] + fib.y * iF[3] + fib.z * iF[6]) + nor.y * (fib.x * iF[1] + fib.y * iF[4] + fib.z * iF[7]) + nor.z * (fib.x * iF[2] + fib.y * iF[5] + fib.z * iF[8]); //iF_nf
    QiFQT[3] = fib.x * (she.x * iF[0] + she.y * iF[3] + she.z * iF[6]) + fib.y * (she.x * iF[1] + she.y * iF[4] + she.z * iF[7]) + fib.z * (she.x * iF[2] + she.y * iF[5] + she.z * iF[8]); //iF_fs
    QiFQT[4] = she.x * (she.x * iF[0] + she.y * iF[3] + she.z * iF[6]) + she.y * (she.x * iF[1] + she.y * iF[4] + she.z * iF[7]) + she.z * (she.x * iF[2] + she.y * iF[5] + she.z * iF[8]); //iF_ss
    QiFQT[5] = nor.x * (she.x * iF[0] + she.y * iF[3] + she.z * iF[6]) + nor.y * (she.x * iF[1] + she.y * iF[4] + she.z * iF[7]) + nor.z * (she.x * iF[2] + she.y * iF[5] + she.z * iF[8]); //iF_ns
    QiFQT[6] = fib.x * (nor.x * iF[0] + nor.y * iF[3] + nor.z * iF[6]) + fib.y * (nor.x * iF[1] + nor.y * iF[4] + nor.z * iF[7]) + fib.z * (nor.x * iF[2] + nor.y * iF[5] + nor.z * iF[8]); //iF_fn
    QiFQT[7] = she.x * (nor.x * iF[0] + nor.y * iF[3] + nor.z * iF[6]) + she.y * (nor.x * iF[1] + nor.y * iF[4] + nor.z * iF[7]) + she.z * (nor.x * iF[2] + nor.y * iF[5] + nor.z * iF[8]); //iF_sn
    QiFQT[8] = nor.x * (nor.x * iF[0] + nor.y * iF[3] + nor.z * iF[6]) + nor.y * (nor.x * iF[1] + nor.y * iF[4] + nor.z * iF[7]) + nor.z * (nor.x * iF[2] + nor.y * iF[5] + nor.z * iF[8]); //iF_nn
 
    constitutive_model->calc_PK1_stress(QiFQT, QFQT, detF, S_PK1);
    
    if(param_globals::Ta.strength != 0)
    {
      apply_active_tension(QFQT, S_PK1);
    }
    // rotate S_PK1 stress back to x,y,z coordinate system
    S_PK1_xyz[0] = fib.x * (fib.x * S_PK1[0] + she.x * S_PK1[3] + nor.x * S_PK1[6]) + she.x * (fib.x * S_PK1[1] + she.x * S_PK1[4] + nor.x * S_PK1[7]) + nor.x * (fib.x * S_PK1[2] + she.x * S_PK1[5] + nor.x * S_PK1[8]); //SPK1_xx
    S_PK1_xyz[1] = fib.y * (fib.x * S_PK1[0] + she.x * S_PK1[3] + nor.x * S_PK1[6]) + she.y * (fib.x * S_PK1[1] + she.x * S_PK1[4] + nor.x * S_PK1[7]) + nor.y * (fib.x * S_PK1[2] + she.x * S_PK1[5] + nor.x * S_PK1[8]); //SPK1_yx
    S_PK1_xyz[2] = fib.z * (fib.x * S_PK1[0] + she.x * S_PK1[3] + nor.x * S_PK1[6]) + she.z * (fib.x * S_PK1[1] + she.x * S_PK1[4] + nor.x * S_PK1[7]) + nor.z * (fib.x * S_PK1[2] + she.x * S_PK1[5] + nor.x * S_PK1[8]); //SPK1_zx
    S_PK1_xyz[3] = fib.x * (fib.y * S_PK1[0] + she.y * S_PK1[3] + nor.y * S_PK1[6]) + she.x * (fib.y * S_PK1[1] + she.y * S_PK1[4] + nor.y * S_PK1[7]) + nor.x * (fib.y * S_PK1[2] + she.y * S_PK1[5] + nor.y * S_PK1[8]); //SPK1_yx
    S_PK1_xyz[4] = fib.y * (fib.y * S_PK1[0] + she.y * S_PK1[3] + nor.y * S_PK1[6]) + she.y * (fib.y * S_PK1[1] + she.y * S_PK1[4] + nor.y * S_PK1[7]) + nor.y * (fib.y * S_PK1[2] + she.y * S_PK1[5] + nor.y * S_PK1[8]); //SPK1_yy
    S_PK1_xyz[5] = fib.z * (fib.y * S_PK1[0] + she.y * S_PK1[3] + nor.y * S_PK1[6]) + she.z * (fib.y * S_PK1[1] + she.y * S_PK1[4] + nor.y * S_PK1[7]) + nor.z * (fib.y * S_PK1[2] + she.y * S_PK1[5] + nor.y * S_PK1[8]); //SPK1_yz
    S_PK1_xyz[6] = fib.x * (fib.z * S_PK1[0] + she.z * S_PK1[3] + nor.z * S_PK1[6]) + she.x * (fib.z * S_PK1[1] + she.z * S_PK1[4] + nor.z * S_PK1[7]) + nor.x * (fib.z * S_PK1[2] + she.z * S_PK1[5] + nor.z * S_PK1[8]); //SPK1_zx
    S_PK1_xyz[7] = fib.y * (fib.z * S_PK1[0] + she.z * S_PK1[3] + nor.z * S_PK1[6]) + she.y * (fib.z * S_PK1[1] + she.z * S_PK1[4] + nor.z * S_PK1[7]) + nor.y * (fib.z * S_PK1[2] + she.z * S_PK1[5] + nor.z * S_PK1[8]); //SPK1_zy
    S_PK1_xyz[8] = fib.z * (fib.z * S_PK1[0] + she.z * S_PK1[3] + nor.z * S_PK1[6]) + she.z * (fib.z * S_PK1[1] + she.z * S_PK1[4] + nor.z * S_PK1[7]) + nor.z * (fib.z * S_PK1[2] + she.z * S_PK1[5] + nor.z * S_PK1[8]); //SPK1_zz
    
    
    double dv = w[iidx] * detJ; //weighted integral
    vol += dv;
      
    for(std::size_t a=0; a < nnodes; a++) //a: node (e.g. {0, 1, 2, 3} for linear tet)
    {
      //derivatives of shape functions
      double sax = gshape[1][a], say = gshape[2][a], saz = gshape[3][a];
      for(std::size_t k=0; k < dpn_ele; k++)
      {
        buff[dpn_ele*a + k] += dv * (S_PK1_xyz[k] * sax
                              + S_PK1_xyz[k + 3] * say
                              + S_PK1_xyz[k + 6] * saz);
      }
    }
  }
}
 
void mech_internal_forces_integrator::dpn(mesh_int_t & dpn)
{
  dpn = 3;
}
 
void mech_stiffness_integrator::operator() (const SF::element_view<mesh_int_t, mesh_real_t> & elem, SF::dmat<double> & buff)
{
  mesh_int_t row_dpn, col_dpn;
  dpn(row_dpn, col_dpn);
  const size_t eidx      = elem.element_index();
  SF::elem_t   type      = elem.type();
  mesh_int_t   nnodes    = elem.num_nodes();
  const int    int_order = get_preferred_int_order(type) - 1;
 
  const int    reg  = material.regionIDs.size() ? material.regionIDs[eidx] : 0;
  const RegionSpecs & reg_spec = material.regions[reg];
 
  assert(reg_spec.material->material_type == MechMat);
  mechMaterial & mat = *static_cast<mechMaterial*>(reg_spec.material);
  Basic_constitutive_model * constitutive_model;
  constitutive_model = constitutive_model_setup(mat.mat_t, param_globals::mech_mat_region[reg].model_params);
  SF::Point fib = elem.fiber(), she = elem.sheet();
 
  bool is_bath   = SF::inner_prod(fib, fib) < 0.01; // bath elements have no fiber direction
  if (is_bath)
  {
    fib = {1.0, 0.0, 0.0};
  }
  bool has_sheet = elem.has_sheet();            // do we have a sheet direction
  bool is_perp = abs(SF::inner_prod(fib, she)) < 0.01;
  if (!has_sheet && is_perp)
  {
    SF::Point helper_dir;
    if(fib.x * fib.x < 0.9)
    {
      helper_dir = {1.0, 0.0, 0.0};
    }
    else
    {
      helper_dir = {0.0, 1.0, 0.0};
    }
    she = cross(fib, helper_dir);
  }
  // normalize fiber, sheet and sheet normal
  normalize(fib);
  normalize(she);
  SF::Point nor = cross(fib, she); //create sheet normal
  normalize(nor);
  double epsilon = 1e-9;
 
  SF::get_transformed_pts(elem, lpts, fib);
 
#ifdef DEBUG_INT
  print_points(lpts, nnodes);
#endif
 
  int ndof = nnodes;
  shape.set_size(4, ndof), gshape.set_size(4, ndof);
  buff.assign(row_dpn * ndof, col_dpn * ndof, 0.0);
 
  int nint = 0;
  elem.integration_points(int_order, ipts, w, nint);
 
  double vol = 0.0;
  
  // Approach: central finite differences
  for (std::size_t i = 0; i < nnodes * row_dpn; i++) {
    vol = 0.0;
    for (std::size_t iidx = 0; iidx < nint; iidx++) {
      //p for + epsilon, n for - epsilon
      double detJ;
      double detF_p;
      double detF_n;
      double F_p[9];
      double iF_p[9];
      double F_n[9];
      double iF_n[9];
      double S_PK1_p[9];
      double S_PK1_n[9];
      
      SF::reference_shape(type, ipts[iidx], shape);
      SF::jacobian_matrix(shape, nnodes, lpts, J);
      SF::invert_jacobian_matrix(type, J, detJ);
      SF::shape_deriv(J, shape, ndof, gshape);
      
      calc_deformation_gradient(elem, iidx, F_p, iF_p, detF_p, i, epsilon);
      calc_deformation_gradient(elem, iidx, F_n, iF_n, detF_n, i, -epsilon);
      
      double QFpQT[9], QiFpQT[9], QFnQT[9], QiFnQT[9]; //rotated deformation gradient and the inverse
      QFpQT[0] = fib.x * (fib.x * F_p[0] + fib.y * F_p[3] + fib.z * F_p[6]) + fib.y * (fib.x * F_p[1] + fib.y * F_p[4] + fib.z * F_p[7]) + fib.z * (fib.x * F_p[2] + fib.y * F_p[5] + fib.z * F_p[8]); //Fp_ff
      QFpQT[1] = she.x * (fib.x * F_p[0] + fib.y * F_p[3] + fib.z * F_p[6]) + she.y * (fib.x * F_p[1] + fib.y * F_p[4] + fib.z * F_p[7]) + she.z * (fib.x * F_p[2] + fib.y * F_p[5] + fib.z * F_p[8]); //Fp_sf
      QFpQT[2] = nor.x * (fib.x * F_p[0] + fib.y * F_p[3] + fib.z * F_p[6]) + nor.y * (fib.x * F_p[1] + fib.y * F_p[4] + fib.z * F_p[7]) + nor.z * (fib.x * F_p[2] + fib.y * F_p[5] + fib.z * F_p[8]); //Fp_nf
      QFpQT[3] = fib.x * (she.x * F_p[0] + she.y * F_p[3] + she.z * F_p[6]) + fib.y * (she.x * F_p[1] + she.y * F_p[4] + she.z * F_p[7]) + fib.z * (she.x * F_p[2] + she.y * F_p[5] + she.z * F_p[8]); //Fp_fs
      QFpQT[4] = she.x * (she.x * F_p[0] + she.y * F_p[3] + she.z * F_p[6]) + she.y * (she.x * F_p[1] + she.y * F_p[4] + she.z * F_p[7]) + she.z * (she.x * F_p[2] + she.y * F_p[5] + she.z * F_p[8]); //Fp_ss
      QFpQT[5] = nor.x * (she.x * F_p[0] + she.y * F_p[3] + she.z * F_p[6]) + nor.y * (she.x * F_p[1] + she.y * F_p[4] + she.z * F_p[7]) + nor.z * (she.x * F_p[2] + she.y * F_p[5] + she.z * F_p[8]); //Fp_ns
      QFpQT[6] = fib.x * (nor.x * F_p[0] + nor.y * F_p[3] + nor.z * F_p[6]) + fib.y * (nor.x * F_p[1] + nor.y * F_p[4] + nor.z * F_p[7]) + fib.z * (nor.x * F_p[2] + nor.y * F_p[5] + nor.z * F_p[8]); //Fp_fn
      QFpQT[7] = she.x * (nor.x * F_p[0] + nor.y * F_p[3] + nor.z * F_p[6]) + she.y * (nor.x * F_p[1] + nor.y * F_p[4] + nor.z * F_p[7]) + she.z * (nor.x * F_p[2] + nor.y * F_p[5] + nor.z * F_p[8]); //Fp_sn
      QFpQT[8] = nor.x * (nor.x * F_p[0] + nor.y * F_p[3] + nor.z * F_p[6]) + nor.y * (nor.x * F_p[1] + nor.y * F_p[4] + nor.z * F_p[7]) + nor.z * (nor.x * F_p[2] + nor.y * F_p[5] + nor.z * F_p[8]); //Fp_nn
      
      QiFpQT[0] = fib.x * (fib.x * iF_p[0] + fib.y * iF_p[3] + fib.z * iF_p[6]) + fib.y * (fib.x * iF_p[1] + fib.y * iF_p[4] + fib.z * iF_p[7]) + fib.z * (fib.x * iF_p[2] + fib.y * iF_p[5] + fib.z * iF_p[8]); //iFp_ff
      QiFpQT[1] = she.x * (fib.x * iF_p[0] + fib.y * iF_p[3] + fib.z * iF_p[6]) + she.y * (fib.x * iF_p[1] + fib.y * iF_p[4] + fib.z * iF_p[7]) + she.z * (fib.x * iF_p[2] + fib.y * iF_p[5] + fib.z * iF_p[8]); //iFp_sf
      QiFpQT[2] = nor.x * (fib.x * iF_p[0] + fib.y * iF_p[3] + fib.z * iF_p[6]) + nor.y * (fib.x * iF_p[1] + fib.y * iF_p[4] + fib.z * iF_p[7]) + nor.z * (fib.x * iF_p[2] + fib.y * iF_p[5] + fib.z * iF_p[8]); //iFp_nf
      QiFpQT[3] = fib.x * (she.x * iF_p[0] + she.y * iF_p[3] + she.z * iF_p[6]) + fib.y * (she.x * iF_p[1] + she.y * iF_p[4] + she.z * iF_p[7]) + fib.z * (she.x * iF_p[2] + she.y * iF_p[5] + she.z * iF_p[8]); //iFp_fs
      QiFpQT[4] = she.x * (she.x * iF_p[0] + she.y * iF_p[3] + she.z * iF_p[6]) + she.y * (she.x * iF_p[1] + she.y * iF_p[4] + she.z * iF_p[7]) + she.z * (she.x * iF_p[2] + she.y * iF_p[5] + she.z * iF_p[8]); //iFp_ss
      QiFpQT[5] = nor.x * (she.x * iF_p[0] + she.y * iF_p[3] + she.z * iF_p[6]) + nor.y * (she.x * iF_p[1] + she.y * iF_p[4] + she.z * iF_p[7]) + nor.z * (she.x * iF_p[2] + she.y * iF_p[5] + she.z * iF_p[8]); //iFp_ns
      QiFpQT[6] = fib.x * (nor.x * iF_p[0] + nor.y * iF_p[3] + nor.z * iF_p[6]) + fib.y * (nor.x * iF_p[1] + nor.y * iF_p[4] + nor.z * iF_p[7]) + fib.z * (nor.x * iF_p[2] + nor.y * iF_p[5] + nor.z * iF_p[8]); //iFp_fn
      QiFpQT[7] = she.x * (nor.x * iF_p[0] + nor.y * iF_p[3] + nor.z * iF_p[6]) + she.y * (nor.x * iF_p[1] + nor.y * iF_p[4] + nor.z * iF_p[7]) + she.z * (nor.x * iF_p[2] + nor.y * iF_p[5] + nor.z * iF_p[8]); //iFp_sn
      QiFpQT[8] = nor.x * (nor.x * iF_p[0] + nor.y * iF_p[3] + nor.z * iF_p[6]) + nor.y * (nor.x * iF_p[1] + nor.y * iF_p[4] + nor.z * iF_p[7]) + nor.z * (nor.x * iF_p[2] + nor.y * iF_p[5] + nor.z * iF_p[8]); //iFp_nn
      
      QFnQT[0] = fib.x * (fib.x * F_n[0] + fib.y * F_n[3] + fib.z * F_n[6]) + fib.y * (fib.x * F_n[1] + fib.y * F_n[4] + fib.z * F_n[7]) + fib.z * (fib.x * F_n[2] + fib.y * F_n[5] + fib.z * F_n[8]); //Fn_ff
      QFnQT[1] = she.x * (fib.x * F_n[0] + fib.y * F_n[3] + fib.z * F_n[6]) + she.y * (fib.x * F_n[1] + fib.y * F_n[4] + fib.z * F_n[7]) + she.z * (fib.x * F_n[2] + fib.y * F_n[5] + fib.z * F_n[8]); //Fn_sf
      QFnQT[2] = nor.x * (fib.x * F_n[0] + fib.y * F_n[3] + fib.z * F_n[6]) + nor.y * (fib.x * F_n[1] + fib.y * F_n[4] + fib.z * F_n[7]) + nor.z * (fib.x * F_n[2] + fib.y * F_n[5] + fib.z * F_n[8]); //Fn_nf
      QFnQT[3] = fib.x * (she.x * F_n[0] + she.y * F_n[3] + she.z * F_n[6]) + fib.y * (she.x * F_n[1] + she.y * F_n[4] + she.z * F_n[7]) + fib.z * (she.x * F_n[2] + she.y * F_n[5] + she.z * F_n[8]); //Fn_fs
      QFnQT[4] = she.x * (she.x * F_n[0] + she.y * F_n[3] + she.z * F_n[6]) + she.y * (she.x * F_n[1] + she.y * F_n[4] + she.z * F_n[7]) + she.z * (she.x * F_n[2] + she.y * F_n[5] + she.z * F_n[8]); //Fn_ss
      QFnQT[5] = nor.x * (she.x * F_n[0] + she.y * F_n[3] + she.z * F_n[6]) + nor.y * (she.x * F_n[1] + she.y * F_n[4] + she.z * F_n[7]) + nor.z * (she.x * F_n[2] + she.y * F_n[5] + she.z * F_n[8]); //Fn_ns
      QFnQT[6] = fib.x * (nor.x * F_n[0] + nor.y * F_n[3] + nor.z * F_n[6]) + fib.y * (nor.x * F_n[1] + nor.y * F_n[4] + nor.z * F_n[7]) + fib.z * (nor.x * F_n[2] + nor.y * F_n[5] + nor.z * F_n[8]); //Fn_fn
      QFnQT[7] = she.x * (nor.x * F_n[0] + nor.y * F_n[3] + nor.z * F_n[6]) + she.y * (nor.x * F_n[1] + nor.y * F_n[4] + nor.z * F_n[7]) + she.z * (nor.x * F_n[2] + nor.y * F_n[5] + nor.z * F_n[8]); //Fn_sn
      QFnQT[8] = nor.x * (nor.x * F_n[0] + nor.y * F_n[3] + nor.z * F_n[6]) + nor.y * (nor.x * F_n[1] + nor.y * F_n[4] + nor.z * F_n[7]) + nor.z * (nor.x * F_n[2] + nor.y * F_n[5] + nor.z * F_n[8]); //Fn_nn
      
      QiFnQT[0] = fib.x * (fib.x * iF_n[0] + fib.y * iF_n[3] + fib.z * iF_n[6]) + fib.y * (fib.x * iF_n[1] + fib.y * iF_n[4] + fib.z * iF_n[7]) + fib.z * (fib.x * iF_n[2] + fib.y * iF_n[5] + fib.z * iF_n[8]); //iFn_ff
      QiFnQT[1] = she.x * (fib.x * iF_n[0] + fib.y * iF_n[3] + fib.z * iF_n[6]) + she.y * (fib.x * iF_n[1] + fib.y * iF_n[4] + fib.z * iF_n[7]) + she.z * (fib.x * iF_n[2] + fib.y * iF_n[5] + fib.z * iF_n[8]); //iFn_sf
      QiFnQT[2] = nor.x * (fib.x * iF_n[0] + fib.y * iF_n[3] + fib.z * iF_n[6]) + nor.y * (fib.x * iF_n[1] + fib.y * iF_n[4] + fib.z * iF_n[7]) + nor.z * (fib.x * iF_n[2] + fib.y * iF_n[5] + fib.z * iF_n[8]); //iFn_nf
      QiFnQT[3] = fib.x * (she.x * iF_n[0] + she.y * iF_n[3] + she.z * iF_n[6]) + fib.y * (she.x * iF_n[1] + she.y * iF_n[4] + she.z * iF_n[7]) + fib.z * (she.x * iF_n[2] + she.y * iF_n[5] + she.z * iF_n[8]); //iFn_fs
      QiFnQT[4] = she.x * (she.x * iF_n[0] + she.y * iF_n[3] + she.z * iF_n[6]) + she.y * (she.x * iF_n[1] + she.y * iF_n[4] + she.z * iF_n[7]) + she.z * (she.x * iF_n[2] + she.y * iF_n[5] + she.z * iF_n[8]); //iFn_ss
      QiFnQT[5] = nor.x * (she.x * iF_n[0] + she.y * iF_n[3] + she.z * iF_n[6]) + nor.y * (she.x * iF_n[1] + she.y * iF_n[4] + she.z * iF_n[7]) + nor.z * (she.x * iF_n[2] + she.y * iF_n[5] + she.z * iF_n[8]); //iFn_ns
      QiFnQT[6] = fib.x * (nor.x * iF_n[0] + nor.y * iF_n[3] + nor.z * iF_n[6]) + fib.y * (nor.x * iF_n[1] + nor.y * iF_n[4] + nor.z * iF_n[7]) + fib.z * (nor.x * iF_n[2] + nor.y * iF_n[5] + nor.z * iF_n[8]); //iFn_fn
      QiFnQT[7] = she.x * (nor.x * iF_n[0] + nor.y * iF_n[3] + nor.z * iF_n[6]) + she.y * (nor.x * iF_n[1] + nor.y * iF_n[4] + nor.z * iF_n[7]) + she.z * (nor.x * iF_n[2] + nor.y * iF_n[5] + nor.z * iF_n[8]); //iFn_sn
      QiFnQT[8] = nor.x * (nor.x * iF_n[0] + nor.y * iF_n[3] + nor.z * iF_n[6]) + nor.y * (nor.x * iF_n[1] + nor.y * iF_n[4] + nor.z * iF_n[7]) + nor.z * (nor.x * iF_n[2] + nor.y * iF_n[5] + nor.z * iF_n[8]); //iFn_nn
      
      constitutive_model->calc_PK1_stress(QiFpQT, QFpQT, detF_p, S_PK1_p);
      constitutive_model->calc_PK1_stress(QiFnQT, QFnQT, detF_n, S_PK1_n);
      
      if(param_globals::Ta.strength != 0)
      {
        apply_active_tension(QFpQT, S_PK1_p);
        apply_active_tension(QFnQT, S_PK1_n);
      }
      // rotate the stress tensors back to x,y,z_p
      double S_PK1_p_xyz[9], S_PK1_n_xyz[9];
      S_PK1_p_xyz[0] = fib.x * (fib.x * S_PK1_p[0] + she.x * S_PK1_p[3] + nor.x * S_PK1_p[6]) + she.x * (fib.x * S_PK1_p[1] + she.x * S_PK1_p[4] + nor.x * S_PK1_p[7]) + nor.x * (fib.x * S_PK1_p[2] + she.x * S_PK1_p[5] + nor.x * S_PK1_p[8]); //SPK1p_xx
      S_PK1_p_xyz[1] = fib.y * (fib.x * S_PK1_p[0] + she.x * S_PK1_p[3] + nor.x * S_PK1_p[6]) + she.y * (fib.x * S_PK1_p[1] + she.x * S_PK1_p[4] + nor.x * S_PK1_p[7]) + nor.y * (fib.x * S_PK1_p[2] + she.x * S_PK1_p[5] + nor.x * S_PK1_p[8]); //SPK1p_yx
      S_PK1_p_xyz[2] = fib.z * (fib.x * S_PK1_p[0] + she.x * S_PK1_p[3] + nor.x * S_PK1_p[6]) + she.z * (fib.x * S_PK1_p[1] + she.x * S_PK1_p[4] + nor.x * S_PK1_p[7]) + nor.z * (fib.x * S_PK1_p[2] + she.x * S_PK1_p[5] + nor.x * S_PK1_p[8]); //SPK1p_zx
      S_PK1_p_xyz[3] = fib.x * (fib.y * S_PK1_p[0] + she.y * S_PK1_p[3] + nor.y * S_PK1_p[6]) + she.x * (fib.y * S_PK1_p[1] + she.y * S_PK1_p[4] + nor.y * S_PK1_p[7]) + nor.x * (fib.y * S_PK1_p[2] + she.y * S_PK1_p[5] + nor.y * S_PK1_p[8]); //SPK1p_yx
      S_PK1_p_xyz[4] = fib.y * (fib.y * S_PK1_p[0] + she.y * S_PK1_p[3] + nor.y * S_PK1_p[6]) + she.y * (fib.y * S_PK1_p[1] + she.y * S_PK1_p[4] + nor.y * S_PK1_p[7]) + nor.y * (fib.y * S_PK1_p[2] + she.y * S_PK1_p[5] + nor.y * S_PK1_p[8]); //SPK1p_yy
      S_PK1_p_xyz[5] = fib.z * (fib.y * S_PK1_p[0] + she.y * S_PK1_p[3] + nor.y * S_PK1_p[6]) + she.z * (fib.y * S_PK1_p[1] + she.y * S_PK1_p[4] + nor.y * S_PK1_p[7]) + nor.z * (fib.y * S_PK1_p[2] + she.y * S_PK1_p[5] + nor.y * S_PK1_p[8]); //SPK1p_yz
      S_PK1_p_xyz[6] = fib.x * (fib.z * S_PK1_p[0] + she.z * S_PK1_p[3] + nor.z * S_PK1_p[6]) + she.x * (fib.z * S_PK1_p[1] + she.z * S_PK1_p[4] + nor.z * S_PK1_p[7]) + nor.x * (fib.z * S_PK1_p[2] + she.z * S_PK1_p[5] + nor.z * S_PK1_p[8]); //SPK1p_zx
      S_PK1_p_xyz[7] = fib.y * (fib.z * S_PK1_p[0] + she.z * S_PK1_p[3] + nor.z * S_PK1_p[6]) + she.y * (fib.z * S_PK1_p[1] + she.z * S_PK1_p[4] + nor.z * S_PK1_p[7]) + nor.y * (fib.z * S_PK1_p[2] + she.z * S_PK1_p[5] + nor.z * S_PK1_p[8]); //SPK1p_zy
      S_PK1_p_xyz[8] = fib.z * (fib.z * S_PK1_p[0] + she.z * S_PK1_p[3] + nor.z * S_PK1_p[6]) + she.z * (fib.z * S_PK1_p[1] + she.z * S_PK1_p[4] + nor.z * S_PK1_p[7]) + nor.z * (fib.z * S_PK1_p[2] + she.z * S_PK1_p[5] + nor.z * S_PK1_p[8]); //SPK1p_zz
      
      S_PK1_n_xyz[0] = fib.x * (fib.x * S_PK1_n[0] + she.x * S_PK1_n[3] + nor.x * S_PK1_n[6]) + she.x * (fib.x * S_PK1_n[1] + she.x * S_PK1_n[4] + nor.x * S_PK1_n[7]) + nor.x * (fib.x * S_PK1_n[2] + she.x * S_PK1_n[5] + nor.x * S_PK1_n[8]); //SPK1n_xx
      S_PK1_n_xyz[1] = fib.y * (fib.x * S_PK1_n[0] + she.x * S_PK1_n[3] + nor.x * S_PK1_n[6]) + she.y * (fib.x * S_PK1_n[1] + she.x * S_PK1_n[4] + nor.x * S_PK1_n[7]) + nor.y * (fib.x * S_PK1_n[2] + she.x * S_PK1_n[5] + nor.x * S_PK1_n[8]); //SPK1n_yx
      S_PK1_n_xyz[2] = fib.z * (fib.x * S_PK1_n[0] + she.x * S_PK1_n[3] + nor.x * S_PK1_n[6]) + she.z * (fib.x * S_PK1_n[1] + she.x * S_PK1_n[4] + nor.x * S_PK1_n[7]) + nor.z * (fib.x * S_PK1_n[2] + she.x * S_PK1_n[5] + nor.x * S_PK1_n[8]); //SPK1n_zx
      S_PK1_n_xyz[3] = fib.x * (fib.y * S_PK1_n[0] + she.y * S_PK1_n[3] + nor.y * S_PK1_n[6]) + she.x * (fib.y * S_PK1_n[1] + she.y * S_PK1_n[4] + nor.y * S_PK1_n[7]) + nor.x * (fib.y * S_PK1_n[2] + she.y * S_PK1_n[5] + nor.y * S_PK1_n[8]); //SPK1n_yx
      S_PK1_n_xyz[4] = fib.y * (fib.y * S_PK1_n[0] + she.y * S_PK1_n[3] + nor.y * S_PK1_n[6]) + she.y * (fib.y * S_PK1_n[1] + she.y * S_PK1_n[4] + nor.y * S_PK1_n[7]) + nor.y * (fib.y * S_PK1_n[2] + she.y * S_PK1_n[5] + nor.y * S_PK1_n[8]); //SPK1n_yy
      S_PK1_n_xyz[5] = fib.z * (fib.y * S_PK1_n[0] + she.y * S_PK1_n[3] + nor.y * S_PK1_n[6]) + she.z * (fib.y * S_PK1_n[1] + she.y * S_PK1_n[4] + nor.y * S_PK1_n[7]) + nor.z * (fib.y * S_PK1_n[2] + she.y * S_PK1_n[5] + nor.y * S_PK1_n[8]); //SPK1n_yz
      S_PK1_n_xyz[6] = fib.x * (fib.z * S_PK1_n[0] + she.z * S_PK1_n[3] + nor.z * S_PK1_n[6]) + she.x * (fib.z * S_PK1_n[1] + she.z * S_PK1_n[4] + nor.z * S_PK1_n[7]) + nor.x * (fib.z * S_PK1_n[2] + she.z * S_PK1_n[5] + nor.z * S_PK1_n[8]); //SPK1n_zx
      S_PK1_n_xyz[7] = fib.y * (fib.z * S_PK1_n[0] + she.z * S_PK1_n[3] + nor.z * S_PK1_n[6]) + she.y * (fib.z * S_PK1_n[1] + she.z * S_PK1_n[4] + nor.z * S_PK1_n[7]) + nor.y * (fib.z * S_PK1_n[2] + she.z * S_PK1_n[5] + nor.z * S_PK1_n[8]); //SPK1n_zy
      S_PK1_n_xyz[8] = fib.z * (fib.z * S_PK1_n[0] + she.z * S_PK1_n[3] + nor.z * S_PK1_n[6]) + she.z * (fib.z * S_PK1_n[1] + she.z * S_PK1_n[4] + nor.z * S_PK1_n[7]) + nor.z * (fib.z * S_PK1_n[2] + she.z * S_PK1_n[5] + nor.z * S_PK1_n[8]); //SPK1n_zz
      
      
      double dv = w[iidx] * detJ; //weighted integral
      vol += dv;
      
      for (std::size_t a = 0; a < nnodes; a++)
      {
        double sax = gshape[1][a], say = gshape[2][a], saz = gshape[3][a];
        for(std::size_t k = 0; k < col_dpn; k++)
        {
          buff[i][3 * a + k] += dv/(2 * epsilon) * ((S_PK1_p_xyz[k] - S_PK1_n_xyz[k]) * sax
                                                    + (S_PK1_p_xyz[k + 3] - S_PK1_n_xyz[k + 3]) * say
                                                    + (S_PK1_p_xyz[k + 6] - S_PK1_n_xyz[k + 6]) * saz);
        }
      }
    }
  }
 
#ifdef DEBUG_INT
  printf("\n vol: %g \n\n", vol);
  buff.disp("Element Matrix:");
#endif
}
 
void mech_stiffness_integrator::dpn(mesh_int_t & row_dpn, mesh_int_t & col_dpn)
{
  row_dpn = 3;
  col_dpn = 3;
}
 
void mech_mass_integrator::operator() (const SF::element_view<mesh_int_t, mesh_real_t> & elem, SF::dmat<double> & buff)
{
  SF::elem_t type      = elem.type();
  mesh_int_t nnodes    = elem.num_nodes();
  const int  int_order = get_preferred_int_order(type);
 
  int ndof = nnodes;
  shape.set_size(4, ndof);
  buff.assign(ndof, ndof, 0.0);
 
  SF::Point fib = {1, 0, 0};  // this is a dummy fibre to satisfy get_transformed_pts()
 
  int nint;
  elem.integration_points(int_order, ipts, w, nint);
  SF::get_transformed_pts(elem, lpts, fib);
 
  // Integration loop
  for(int iidx=0; iidx < nint; iidx++)
  {
    double detj;
    // get shape function and its spatial derivatives
    SF::reference_shape(type, ipts[iidx], shape);
    SF::jacobian_matrix(shape, nnodes, lpts, J);
    SF::invert_jacobian_matrix(type, J, detj);
 
    double dx = w[iidx] * detj;
 
    for (int k = 0; k < ndof; k++) {
      double sk = shape[0][k];
      for (int l = 0; l < ndof; l++) {
        double sl = shape[0][l];
        buff[k][l] += sk * sl * dx;
      }
    }
  }
}
 
void mech_mass_integrator::dpn(mesh_int_t & row_dpn, mesh_int_t & col_dpn)
{
  row_dpn = 3;
  col_dpn = 3;
}
 
}  // namespace opencarp