SF_fem_utils.h

Go to the documentation of this file.

// SPDX-FileCopyrightText: Copyright (c) NumeriCor GmbH
// SPDX-License-Identifier: Apache-2.0
 
#ifndef _SF_FEM_H
#define _SF_FEM_H
 
#include <cassert>
#include <cmath>
#include <cstring>
#include <mpi.h>
 
#define SF_MAX_ELEM_NODES 10  
 
#include "SF_container.h"
 
#include "SF_abstract_vector.h"
#include "SF_abstract_matrix.h"
#include "SF_abstract_lin_solver.h"
 
namespace SF {
 
inline short num_dof(elem_t type, short order)
{
  short ndof = -1;
  switch(type) {
    case Line:
      if (order == 1)      ndof = 2;
      else if (order == 2) ndof = 3;
      else {
        fprintf(stderr, "Line element order %d not implemented yet.\n", order);
        exit(1);
      }
      break;
    case Tri:
      if (order == 1)      ndof = 3;
      else if (order == 2) ndof = 6;
      else {
        fprintf(stderr, "Tri element order %d not implemented yet.\n", order);
        exit(1);
      }
      break;
    case Quad:
      if (order == 1)      ndof = 4;
      else if (order == 2) ndof = 8;
      else {
        fprintf(stderr, "Quad element order %d not implemented yet.\n", order);
        exit(1);
      }
      break;
    case Tetra:
      if (order == 1)      ndof = 4;
      else if (order == 2) ndof = 10;
      else {
        fprintf(stderr, "Tetra element order %d not implemented yet.\n", order);
        exit(1);
      }
      break;
    case Hexa:
      if (order == 1)      ndof = 8;
      else if (order == 2) ndof = 20; // serendipity element
      else {
        fprintf(stderr, "Hexa element order %d not implemented yet.\n", order);
        exit(1);
      }
      break;
 
    default:
        fprintf(stderr, "%s error: Unsupported element type.\n", __func__);
        exit(1);
  }
 
  return ndof;
}
 
 
inline void general_integration_points(const elem_t type,
                                const short order,
                                Point* ip,
                                double* w,
                                int & nint)
{
  switch (type) {
    case Line:
      if (order == 1) {
        ip[0].x = 0.; ip[0].y = 0.; ip[0].z = 0.;
        w[0] = 2.; nint = 1;
      } else if (order == 2) {
        const double sqrt3 = 0.577350269189626;  //=sqrt(1./3.)
        ip[0].x = -sqrt3; ip[0].y = 0.; ip[0].z = 0.;
        ip[1].x = +sqrt3; ip[1].y = 0.; ip[1].z = 0.;
        w[0] = 1.; w[1] = 1.; nint = 2;
      }
      break;
 
    case Tri:
      if (order == 1) {
        ip[0].x = 1. / 3.; ip[0].y = 1. / 3.; ip[0].z = 0.;
        w[0] = 1. / 2.;
        nint = 1;
      } else if (order == 2) {
        ip[0].x = 1. / 6.; ip[0].y = 1. / 6.; ip[0].z = 0.;
        ip[1].x = 4. / 6.; ip[1].y = 1. / 6.; ip[1].z = 0.;
        ip[2].x = 1. / 6.; ip[2].y = 4. / 6.; ip[2].z = 0.;
        w[0] = 1. / 6.; w[1] = 1. / 6.; w[2] = 1. / 6.;
        nint = 3;
      } else if (order == 3 || order == 4) {
        // Gauss computed
        ip[0].x = 0.188409405952072339650, ip[0].y = 0.787659461760847001700, ip[0].z = 0;
        ip[1].x = 0.523979067720100721850, ip[1].y = 0.409466864440734712450, ip[1].z = 0;
        ip[2].x = 0.808694385677669824730, ip[2].y = 0.088587959512703928766, ip[2].z = 0;
        ip[3].x = 0.106170269119576471390, ip[3].y = 0.787659461760847001700, ip[3].z = 0;
        ip[4].x = 0.295266567779632616020, ip[4].y = 0.409466864440734712450, ip[4].z = 0;
        ip[5].x = 0.455706020243648035620, ip[5].y = 0.088587959512703928766, ip[5].z = 0;
        ip[6].x = 0.023931132287080617016, ip[6].y = 0.787659461760847001700, ip[6].z = 0;
        ip[7].x = 0.066554067839164496312, ip[7].y = 0.409466864440734712450, ip[7].z = 0;
        ip[8].x = 0.102717654809626260380, ip[8].y = 0.088587959512703928766, ip[8].z = 0;
 
        w[0] = 0.019396383305959434551;
        w[1] = 0.063678085099884929043;
        w[2] = 0.055814420483044288601;
        w[3] = 0.03103421328953510222;
        w[4] = 0.1018849361598159059;
        w[5] = 0.089303072772870889517;
        w[6] = 0.019396383305959434551;
        w[7] = 0.063678085099884929043;
        w[8] = 0.055814420483044288601;
        nint = 9;
      }
      break;
 
    case Quad:
      if (order == 1) {
        ip[0].x = 0.; ip[0].y = 0.; ip[0].z = 0.;
        w[0] = 4.;
        nint = 1;
      } else if (order == 2 || order == 3) {
        const double sqrt3 = 0.577350269189626;  //=sqrt(1./3.)
        ip[0].x = -sqrt3; ip[0].y = -sqrt3; ip[0].z = 0.;
        ip[1].x = +sqrt3; ip[1].y = -sqrt3; ip[1].z = 0.;
        ip[2].x = +sqrt3; ip[2].y = +sqrt3; ip[2].z = 0.;
        ip[3].x = -sqrt3; ip[3].y = +sqrt3; ip[3].z = 0.;
        w[0] = 1.; w[1] = 1.; w[2] = 1.; w[3] = 1.;
        nint = 4;
      } else if (order == 4) {
        // Gauss computed
        ip[0].x = 0.88729833462074170214, ip[0].y = 0.88729833462074170214, ip[0].z = 0;
        ip[1].x = 0.88729833462074170214, ip[1].y = 0.5,                    ip[1].z = 0;
        ip[2].x = 0.88729833462074170214, ip[2].y = 0.11270166537925829786, ip[2].z = 0;
        ip[3].x = 0.5,                    ip[3].y = 0.88729833462074170214, ip[3].z = 0;
        ip[4].x = 0.5,                    ip[4].y = 0.5,                    ip[4].z = 0;
        ip[5].x = 0.5,                    ip[5].y = 0.11270166537925829786, ip[5].z = 0;
        ip[6].x = 0.11270166537925829786, ip[6].y = 0.88729833462074170214, ip[6].z = 0;
        ip[7].x = 0.11270166537925829786, ip[7].y = 0.5,                    ip[7].z = 0;
        ip[8].x = 0.11270166537925829786, ip[8].y = 0.11270166537925829786, ip[8].z = 0;
 
        w[0] = 0.077160493827160225866;
        w[1] = 0.12345679012345638081;
        w[2] = 0.077160493827160225866;
        w[3] = 0.12345679012345638081;
        w[4] = 0.19753086419753024261;
        w[5] = 0.12345679012345638081;
        w[6] = 0.077160493827160225866;
        w[7] = 0.12345679012345638081;
        w[8] = 0.077160493827160225866;
 
        nint = 9;
      }
      break;
 
    case Tetra:
      if (order == 0 || order == 1) {
        // exact for linears
        ip[0].x = 0.25; ip[0].y = 0.25; ip[0].z = 0.25;
        w[0] = .16666666666666666666;
        nint = 1;
      } else if (order == 2) {
        // exact for quadratics
        double gauss1 = 0.13819660112501051518; // (5-sqrt(5))/20
        double gauss2 = 0.58541019662496845446; // (5+3sqrt(5))/20
        double weight = 0.0416666666666666666666666666666666; // 1/24
        ip[0].x = gauss1; ip[0].y = gauss1; ip[0].z = gauss1;
        ip[1].x = gauss2; ip[1].y = gauss1; ip[1].z = gauss1;
        ip[2].x = gauss1; ip[2].y = gauss2; ip[2].z = gauss1;
        ip[3].x = gauss1; ip[3].y = gauss1; ip[3].z = gauss2;
        w[0] = weight; w[1] = weight; w[2] = weight; w[3] = weight;
        nint = 4;
      } else if (order == 3 || order == 4) {
        // Gauss computed
        ip[0].x = 0.12764656212038541505,  ip[0].y = 0.29399880063162286969,  ip[0].z = 0.54415184401122529412;
        ip[1].x = 0.2457133252117133515,   ip[1].y = 0.56593316507280100325,  ip[1].z = 0.12251482265544139105;
        ip[2].x = 0.30377276481470755209,  ip[2].y = 0.070679724159396897787, ip[2].z = 0.54415184401122529412;
        ip[3].x = 0.58474756320489440498,  ip[3].y = 0.13605497680284600603,  ip[3].z = 0.12251482265544139105;
        ip[4].x = 0.034202793236766414198, ip[4].y = 0.29399880063162286969,  ip[4].z = 0.54415184401122529412;
        ip[5].x = 0.065838687060044420729, ip[5].y = 0.56593316507280100325,  ip[5].z = 0.12251482265544139105;
        ip[6].x = 0.081395667014670256001, ip[6].y = 0.070679724159396897787, ip[6].z = 0.54415184401122529412;
        ip[7].x = 0.15668263733681833672,  ip[7].y = 0.13605497680284600603,  ip[7].z = 0.12251482265544139105;
 
        w[0] = 0.0091694299214797256314;
        w[1] = 0.0211570064545240181520;
        w[2] = 0.0160270405984766287080;
        w[3] = 0.0369798563588529458080;
        w[4] = 0.0091694299214797291009;
        w[5] = 0.0211570064545240285600;
        w[6] = 0.0160270405984766356470;
        w[7] = 0.0369798563588529666250;
 
        nint = 8;
      }
      break;
 
    case Hexa:
      if (order == 0) {
        ip[0].x = 0.; ip[0].y = 0.; ip[0].z = 0.;
        w[0] = 8.;
        nint = 1;
      } else if (order == 1 || order == 2) {
        const double sqrt3 = 0.577350269189626;  //=sqrt(1./3.)
        ip[0].x = -sqrt3; ip[0].y = -sqrt3; ip[0].z = -sqrt3;
        ip[1].x = +sqrt3; ip[1].y = -sqrt3; ip[1].z = -sqrt3;
        ip[2].x = +sqrt3; ip[2].y = +sqrt3; ip[2].z = -sqrt3;
        ip[3].x = -sqrt3; ip[3].y = +sqrt3; ip[3].z = -sqrt3;
        ip[4].x = -sqrt3; ip[4].y = -sqrt3; ip[4].z = +sqrt3;
        ip[5].x = +sqrt3; ip[5].y = -sqrt3; ip[5].z = +sqrt3;
        ip[6].x = +sqrt3; ip[6].y = +sqrt3; ip[6].z = +sqrt3;
        ip[7].x = -sqrt3; ip[7].y = +sqrt3; ip[7].z = +sqrt3;
        w[0] = 1.; w[1] = 1.; w[2] = 1.; w[3] = 1.;
        w[4] = 1.; w[5] = 1.; w[6] = 1.; w[7] = 1.;
        nint = 8;
      }
      break;
 
    case Prism:
      if (order == 0) {
        ip[0].x = 0.33333333333333331, ip[0].y = 0.33333333333333337, ip[0].z = 0.5;
        w[0] = 0.5;
        nint = 1;
      } else if(order == 1 || order == 2) {
#if 1
        ip[0].x = 0.8168475628; ip[0].y = 0.0915762112; ip[0].z = 0.2113248706;
        ip[1].x = 0.8168475628; ip[1].y = 0.0915762112; ip[1].z = 0.7886751294;
        ip[2].x = 0.0915762112; ip[2].y = 0.8168475628; ip[2].z = 0.2113248706;
        ip[3].x = 0.0915762112; ip[3].y = 0.8168475628; ip[3].z = 0.7886751294;
        ip[4].x = 0.0915762112; ip[4].y = 0.0915762112; ip[4].z = 0.2113248706;
        ip[5].x = 0.0915762112; ip[5].y = 0.0915762112; ip[5].z = 0.7886751294;
        w[0] = 0.0833333358;
        w[1] = 0.0833333358;
        w[2] = 0.0833333358;
        w[3] = 0.0833333358;
        w[4] = 0.0833333358;
        w[5] = 0.0833333358;
        nint = 6;
#else
        ip[0].x = 0.28001991549907407,  ip[0].y = 0.64494897427831788, ip[0].z = 0.78867513459481287;
        ip[1].x = 0.28001991549907407,  ip[1].y = 0.64494897427831788, ip[1].z = 0.21132486540518713;
        ip[2].x = 0.66639024601470143,  ip[2].y = 0.15505102572168217, ip[2].z = 0.78867513459481287;
        ip[3].x = 0.66639024601470143,  ip[3].y = 0.15505102572168217, ip[3].z = 0.21132486540518713;
        ip[4].x = 0.075031110222608124, ip[4].y = 0.64494897427831788, ip[4].z = 0.78867513459481287;
        ip[5].x = 0.075031110222608124, ip[5].y = 0.64494897427831788, ip[5].z = 0.21132486540518713;
        ip[6].x = 0.17855872826361643,  ip[6].y = 0.15505102572168217, ip[6].z = 0.78867513459481287;
        ip[7].x = 0.17855872826361643,  ip[7].y = 0.15505102572168217, ip[7].z = 0.21132486540518713;
        w[0] = 0.045489654564005534;
        w[1] = 0.045489654564005548;
        w[2] = 0.079510345435994237;
        w[3] = 0.079510345435994265;
        w[4] = 0.045489654564005548;
        w[5] = 0.045489654564005562;
        w[6] = 0.079510345435994265;
        w[7] = 0.079510345435994292;
        nint = 8;
#endif
      }
      break;
 
    case Pyramid:
      if (order == 1) {
        ip[0].x = -0.433013; ip[0].y = -0.433013; ip[0].z = 0.25;
        ip[1].x = 0.433013;  ip[1].y = -0.433013; ip[1].z = 0.25;
        ip[2].x = 0.433013;  ip[2].y = 0.433013;  ip[2].z = 0.25;
        ip[3].x = -0.433013; ip[3].y = 0.433013;  ip[3].z = 0.25;
        w[0] = 1. / 3;
        w[1] = 1. / 3;
        w[2] = 1. / 3;
        w[3] = 1. / 3;
        nint = 4;
      } else if (order == 2) {
        ip[0 ].x = 0.040086493940919059,  ip[0 ].y = 0.040086493940919059,  ip[0 ].z = 0.93056815579702623;
        ip[1 ].x = 0.19053106107826956,   ip[1 ].y = 0.19053106107826956,   ip[1 ].z = 0.66999052179242813;
        ip[2 ].x = 0.38681920811135617,   ip[2 ].y = 0.38681920811135617,   ip[2 ].z = 0.33000947820757187;
        ip[3 ].x = 0.53726377524870672,   ip[3 ].y = 0.53726377524870672,   ip[3 ].z = 0.069431844202973714;
        ip[4 ].x = 0.040086493940919059,  ip[4 ].y = -0.040086493940919059, ip[4 ].z = 0.93056815579702623;
        ip[5 ].x = 0.19053106107826956,   ip[5 ].y = -0.19053106107826956,  ip[5 ].z = 0.66999052179242813;
        ip[6 ].x = 0.38681920811135617,   ip[6 ].y = -0.38681920811135617,  ip[6 ].z = 0.33000947820757187;
        ip[7 ].x = 0.53726377524870672,   ip[7 ].y = -0.53726377524870672,  ip[7 ].z = 0.069431844202973714;
        ip[8 ].x = -0.040086493940919059, ip[8 ].y = 0.040086493940919059,  ip[8 ].z = 0.93056815579702623;
        ip[9 ].x = -0.19053106107826956,  ip[9 ].y = 0.19053106107826956,   ip[9 ].z = 0.66999052179242813;
        ip[10].x = -0.38681920811135617,  ip[10].y = 0.38681920811135617,   ip[10].z = 0.33000947820757187;
        ip[11].x = -0.53726377524870672,  ip[11].y = 0.53726377524870672,   ip[11].z = 0.069431844202973714;
        ip[12].x = -0.040086493940919059, ip[12].y = -0.040086493940919059, ip[12].z = 0.93056815579702623;
        ip[13].x = -0.19053106107826956,  ip[13].y = -0.19053106107826956,  ip[13].z = 0.66999052179242813;
        ip[14].x = -0.38681920811135617,  ip[14].y = -0.38681920811135617,  ip[14].z = 0.33000947820757187;
        ip[15].x = -0.53726377524870672,  ip[15].y = -0.53726377524870672,  ip[15].z = 0.069431844202973714;
 
        w[0 ] = 0.000838466012258970;
        w[1 ] = 0.035511343496716564;
        w[2 ] = 0.14636983865620462;
        w[3 ] = 0.15061368516815288;
        w[4 ] = 0.000838466012258971;
        w[5 ] = 0.035511343496716585;
        w[6 ] = 0.1463698386562047;
        w[7 ] = 0.15061368516815296;
        w[8 ] = 0.000838466012258971;
        w[9 ] = 0.035511343496716585;
        w[10] = 0.1463698386562047;
        w[11] = 0.15061368516815296;
        w[12] = 0.000838466012258971;
        w[13] = 0.035511343496716599;
        w[14] = 0.14636983865620476;
        w[15] = 0.15061368516815302;
        nint = 16;
      }
      break;
 
    default:
      fprintf(stderr, "%s error: Unsupported element type.\n", __func__);
      exit(1);
  }
}
 
 
inline void reference_shape(const elem_t type,
                            const Point ip,
                            dmat<double> & rshape)
{
  switch (type) {
    case Line:
    {
      rshape[0][0] = 0.5 * (1.0 - ip.x);
      rshape[0][1] = 0.5 * (1.0 + ip.x);
 
      rshape[1][0] = -0.5;
      rshape[1][1] = 0.5;
 
      rshape[2][0] = 0.;
      rshape[2][1] = 0.;
 
      rshape[3][0] = 0.;
      rshape[3][1] = 0.;
      break;
    }
 
    case Tri:
    {
      double lam0 = (1.0 - ip.x - ip.y);
      double lam1 = ip.x;
      double lam2 = ip.y;
 
      rshape[0][0] = lam0;
      rshape[0][1] = lam1;
      rshape[0][2] = lam2;
 
      // derivative w.r.t. xi
      rshape[1][0] = -1.0;
      rshape[1][1] =  1.0;
      rshape[1][2] =  0.0;
 
      // derivative w.r.t. eta
      rshape[2][0] = -1.0;
      rshape[2][1] =  0.0;
      rshape[2][2] =  1.0;
 
      // derivative w.r.t. zeta (is zero)
      rshape[3][0] = 0.;
      rshape[3][1] = 0.;
      rshape[3][2] = 0.;
      break;
    }
 
    case Quad: {
      double qrtr = 0.25;
      const double node[4][2] =
      {
        { -1.0, -1.0},
        { 1.0, -1.0},
        { 1.0,  1.0},
        { -1.0,  1.0}
      };
 
      // adjust to ordering given in carp manual
      int v[4] = {0, 1, 2, 3};
 
      for (int i = 0; i < 4; i++) {
        // shape function
        rshape[0][i] = qrtr * (1. + ip.x * node[v[i]][0]) * (1. + ip.y * node[v[i]][1]);
        // derivative w.r.t. xi
        rshape[1][i] = node[v[i]][0] * qrtr * (1. + ip.y * node[v[i]][1]);
        // derivative w.r.t. eta
        rshape[2][i] = node[v[i]][1] * qrtr * (1. + ip.x * node[v[i]][0]);
        // derivative w.r.t. zeta (is zero)
        rshape[3][i] = 0.;
      }
      break;
    }
 
    case Tetra:
    {
      double lam0 = 1.0 - ip.x - ip.y - ip.z;
      double lam1 = ip.x;
      double lam2 = ip.y;
      double lam3 = ip.z;
      //static int v[10] = {0,3,1,2,7,8,4,6,9,5};
 
      // shape function
      rshape[0][0] = lam0;
      rshape[0][1] = lam1;
      rshape[0][2] = lam2;
      rshape[0][3] = lam3;
 
      // derivative w.r.t. xi
      rshape[1][0] = -1.0;
      rshape[1][1] =  1.0;
      rshape[1][2] =  0.0;
      rshape[1][3] =  0.0;
 
      // derivative w.r.t. eta
      rshape[2][0] = -1.0;
      rshape[2][1] =  0.0;
      rshape[2][2] =  1.0;
      rshape[2][3] =  0.0;
 
      // derivative w.r.t. zeta
      rshape[3][0] = -1.0;
      rshape[3][1] =  0.0;
      rshape[3][2] =  0.0;
      rshape[3][3] =  1.0;
      break;
    }
 
    case Hexa:
    {
      const double oito = 1.0 / 8.0;
      static const double node[8][3] =
      {
        { -1.0, -1.0, -1.0},
        { 1.0, -1.0, -1.0},
        { 1.0,  1.0, -1.0},
        { -1.0,  1.0, -1.0},
 
        { -1.0, -1.0,  1.0},
        { 1.0, -1.0,  1.0},
        { 1.0,  1.0,  1.0},
        { -1.0,  1.0,  1.0}
      };
 
      // adjust to ordering given in carp manual
      static int v[8] = {4, 7, 6, 5, 0, 1, 2, 3};
 
      for (int i = 0; i < 8; i++) {
        // shape function
        rshape[0][i] = oito * (1. + ip.x * node[v[i]][0]) * (1. + ip.y * node[v[i]][1]) * (1. + ip.z * node[v[i]][2]);
        // derivative w.r.t. xi
        rshape[1][i] = node[v[i]][0] * oito * (1. + ip.y * node[v[i]][1]) * (1. + ip.z * node[v[i]][2]);
        // derivative w.r.t. eta
        rshape[2][i] = node[v[i]][1] * oito * (1. + ip.x * node[v[i]][0]) * (1. + ip.z * node[v[i]][2]);
        // derivative w.r.t zeta
        rshape[3][i] = node[v[i]][2] * oito * (1. + ip.x * node[v[i]][0]) * (1. + ip.y * node[v[i]][1]);
      }
 
      break;
    }
 
    case Prism:
    {
      rshape[0][0] = (1.0 - ip.x - ip.y) * ip.z;
      rshape[0][1] = ip.y * ip.z;
      rshape[0][2] = ip.x * ip.z;
      rshape[0][3] = (1.0 - ip.x - ip.y) * (1. - ip.z);
      rshape[0][4] = ip.x * (1. - ip.z);
      rshape[0][5] = ip.y * (1. - ip.z);
 
      // derivative w.r.t. x
      rshape[1][0] = -ip.z;
      rshape[1][1] = 0.0;
      rshape[1][2] = ip.z;
      rshape[1][3] = ip.z - 1.0;
      rshape[1][4] = 1. - ip.z;
      rshape[1][5] = 0.0;
 
      // derivative w.r.t. y
      rshape[2][0] = -ip.z;
      rshape[2][1] = ip.z;
      rshape[2][2] = 0.0;
      rshape[2][3] = ip.z - 1.0;
      rshape[2][4] = 0.0;
      rshape[2][5] = 1. - ip.z;
 
      // derivative w.r.t. z
      rshape[3][0] = 1.0 - ip.x - ip.y;
      rshape[3][1] = ip.y;
      rshape[3][2] = ip.x;
      rshape[3][3] = ip.x + ip.y - 1.0;
      rshape[3][4] = -ip.x;
      rshape[3][5] = -ip.y;
      break;
    }
 
    case Pyramid:
    {
      const double qrtr = 0.25;
      // shape function
      const double lterm0 = ip.x * ip.y * ip.z / (1.0 - ip.z);
      rshape[0][0] = qrtr * ( (1.0 + ip.x) * (1.0 + ip.y) - ip.z + lterm0 );
      rshape[0][1] = qrtr * ( (1.0 - ip.x) * (1.0 + ip.y) - ip.z - lterm0 );
      rshape[0][2] = qrtr * ( (1.0 - ip.x) * (1.0 - ip.y) - ip.z + lterm0 );
      rshape[0][3] = qrtr * ( (1.0 + ip.x) * (1.0 - ip.y) - ip.z - lterm0 );
      rshape[0][4] = ip.z;
 
      // derivative w.r.t. xi
      const double lterm1 = (ip.y * ip.z) / (1.0 - ip.z);
      rshape[1][0] = qrtr * (  (1.0 + ip.y) + lterm1 );
      rshape[1][1] = qrtr * ( -(1.0 + ip.y) - lterm1 );
      rshape[1][2] = qrtr * ( -(1.0 - ip.y) + lterm1 );
      rshape[1][3] = qrtr * (  (1.0 - ip.y) - lterm1 );
      rshape[1][4] = 0.0;
 
      // derivative w.r.t. eta
      const double lterm2 = (ip.x * ip.z) / (1.0 - ip.z);
      rshape[2][0] = qrtr * (  (1.0 + ip.x) + lterm2 );
      rshape[2][1] = qrtr * (  (1.0 - ip.x) - lterm2 );
      rshape[2][2] = qrtr * ( -(1.0 - ip.x) + lterm2 );
      rshape[2][3] = qrtr * ( -(1.0 + ip.x) - lterm2 );
      rshape[2][4] = 0.0;
 
      // derivative w.r.t zeta
      const double lterm3 = ((ip.x * ip.y * ip.z) / (1.0 - ip.z) * (1.0 - ip.z)) + (ip.x * ip.y / (1.0 - ip.z));
      rshape[3][0] = qrtr * ( -1.0 + lterm3 );
      rshape[3][1] = qrtr * ( -1.0 - lterm3 );
      rshape[3][2] = qrtr * ( -1.0 + lterm3 );
      rshape[3][3] = qrtr * ( -1.0 - lterm3 );
      rshape[3][4] = 1.0;
      break;
    }
 
    default:
      fprintf(stderr, "%s: Unimplemented element type! Aborting!\n", __func__);
      exit(1);
  }
}
 
inline void jacobian_matrix(const dmat<double> & rshape,
                        const int npts,
                        const Point* pts,
                        double *J)
{
  // zero the 3x3 jacobian matrix
  memset (J, 0, 9 * sizeof(double) );
  // note that ref_shape hold shape functions in [0][i] and derivatives in [k][i]
  // if element is 2 dimensional then [3][i] is set to 0!
  for (int i = 0; i < npts; i++)
  {
    J[0] += rshape[1][i] * pts[i].x;
    J[1] += rshape[1][i] * pts[i].y;
    J[2] += rshape[1][i] * pts[i].z;
    J[3] += rshape[2][i] * pts[i].x;
    J[4] += rshape[2][i] * pts[i].y;
    J[5] += rshape[2][i] * pts[i].z;
    J[6] += rshape[3][i] * pts[i].x;
    J[7] += rshape[3][i] * pts[i].y;
    J[8] += rshape[3][i] * pts[i].z;
  }
}
 
inline void invert_jacobian_matrix(const elem_t type, double* J, double & detJ)
{
  switch(type)
  {
    // all 3D elems
    default:
    case Tetra:
      invert_3x3(J, detJ);
      break;
 
    // 2D elems
    case Quad:
    case Tri: {
      double J2[4] = {J[0], J[1], J[3], J[4]};
 
      invert_2x2(J2, detJ);
      detJ = fabs(detJ);
 
      J[0] = J2[0]; J[1] = J2[1]; J[2] = 0.0;
      J[3] = J2[2]; J[4] = J2[3]; J[5] = 0.0;
      J[6] = 0.0;   J[7] = 0.0;   J[8] = 0.0;
      break;
    }
 
    // 1D elem
    case Line:
      detJ = J[0];
      J[0] = 1.0 / detJ;
      break;
  }
}
 
inline void shape_deriv(const double *iJ,
                    const dmat<double> & rshape,
                    const int ndof,
                    dmat<double> & shape)
{
  for (int in = 0; in < ndof; in++)
  {
    shape[1][in] = iJ[0] * rshape[1][in] +
                   iJ[1] * rshape[2][in] +
                   iJ[2] * rshape[3][in];
 
    shape[2][in] = iJ[3] * rshape[1][in] +
                   iJ[4] * rshape[2][in] +
                   iJ[5] * rshape[3][in];
 
    shape[3][in] = iJ[6] * rshape[1][in] +
                   iJ[7] * rshape[2][in] +
                   iJ[8] * rshape[3][in];
  }
}
 
template<class T, class S>
class element_view
{
  private:
  const meshdata<T, S> & _mesh;    
  const vector<T> & _glob_numbr;   
  T        _esize;                 
  const T* _offset_con;            
  size_t   _eidx;                  
  int      _rank;                  
 
  public:
  element_view(const meshdata<T, S> & mesh, const SF_nbr nbr) :
               _mesh(mesh), _glob_numbr(_mesh.get_numbering(nbr))
  {
    if(mesh.l_numelem) this->set_elem(0);
    MPI_Comm_rank(_mesh.comm, &_rank);
  }
 
  inline void set_elem(size_t eidx)
  {
    _eidx = eidx;
 
    T offset    = _mesh.dsp[_eidx];
    _esize      = _mesh.dsp[_eidx+1] - offset;
    _offset_con = _mesh.con.data()   + offset;
  }
 
  inline bool next()
  {
    if(_eidx < (_mesh.l_numelem - 1) )
    {
      this->set_elem(_eidx + 1);
      return true;
    }
    else
      return false;
  }
 
  inline T num_nodes() const
  {
    return _esize;
  }
 
  inline elem_t type() const
  {
    return _mesh.type[_eidx];
  }
 
  inline T tag() const
  {
    return _mesh.tag[_eidx];
  }
 
  inline const T & node(short nidx) const
  {
    return _offset_con[nidx];
  }
 
  inline const T & global_node(short nidx) const
  {
    return _glob_numbr[_offset_con[nidx]];
  }
  inline const T & global_node(short nidx, SF_nbr nbr) const
  {
    const vector<T> & n = _mesh.get_numbering(nbr);
    return n[_offset_con[nidx]];
  }
 
  inline const T* nodes() const
  {
    return _offset_con;
  }
  inline Point coord(short nidx) const
  {
    T idx = _offset_con[nidx];
    return {_mesh.xyz[idx*3+0], _mesh.xyz[idx*3+1], _mesh.xyz[idx*3+2]};
  }
 
  inline Point fiber() const
  {
    if(_mesh.fib.size())
      return {_mesh.fib[_eidx*3+0], _mesh.fib[_eidx*3+1], _mesh.fib[_eidx*3+2]};
    else
      return {0,0,0};
  }
  inline Point sheet() const
  {
    if(_mesh.she.size())
      return {_mesh.she[_eidx*3+0], _mesh.she[_eidx*3+1], _mesh.she[_eidx*3+2]};
    else
      return {0,0,0};
  }
 
  bool has_sheet() const
  {
    return _mesh.she.size() > 0;
  }
 
  inline size_t element_index() const
  {
    return _eidx;
  }
 
  inline size_t global_element_index() const
  {
    return _mesh.epl.algebraic_layout()[_rank] + _eidx;
  }
  inline size_t global_element_index(SF_nbr nbr) const
  {
    const vector<T> & en = _mesh.get_numbering(nbr);
    return en[_eidx];
  }
 
 
  inline short num_dof(short order) const
  {
    return SF::num_dof(_mesh.type[_eidx], order);
  }
 
  inline void integration_points(const short order, Point* ip, double* w, int & nint) const
  {
    general_integration_points(_mesh.type[_eidx], order, ip, w, nint);
  }
 
  inline short dimension() const
  {
    switch(_mesh.type[_eidx])
    {
      default:
      case Tetra:
        return 3;
 
      case Tri:
      case Quad:
        return 2;
 
      case Line:
        return 1;
    }
  }
};
 
template<class T, class S>
class matrix_integrator
{
  public:
  virtual void operator() (const element_view<T,S> & elem, dmat<double> & buff) = 0;
  virtual void dpn(T & row_dpn, T & col_dpn) = 0;
};
template<class T, class S>
class vector_integrator
{
  protected:
  void zero_buff(double* buff, T nrows)
  {
    for(T i=0; i<nrows; i++) buff[i] = 0.0;
  }
 
  public:
  virtual void operator() (const element_view<T,S> & elem, double* buff) = 0;
  virtual void dpn(T & dpn) = 0;
};
 
 
template<typename T, typename V> inline
void canonic_indices(const T* nidx,
                     const T* nbr,
                     const T esize,
                     const short dpn,
                     V* cidx)
{
  for(T i=0; i<esize; i++)
    for(short j=0; j<dpn; j++)
      cidx[i*dpn + j] = nbr[nidx[i]]*dpn + j;
}
 
template<class T, class S>
inline void assemble_matrix(abstract_matrix<T,S> & mat,
                            meshdata<mesh_int_t,mesh_real_t> & domain,
                            matrix_integrator<mesh_int_t,mesh_real_t> & integrator)
{
  int rank;
  MPI_Comm_rank(domain.comm, &rank);
 
  // we want to make sure that the element integrator fits the matrix
  mesh_int_t row_dpn, col_dpn;
  integrator.dpn(row_dpn, col_dpn);
  assert(mat.dpn_row() == row_dpn && mat.dpn_col() == col_dpn);
 
  // allocate row / col index buffers
  vector<T> row_idx(SF_MAX_ELEM_NODES * row_dpn), col_idx(SF_MAX_ELEM_NODES * col_dpn);
 
  // allocate elem index buffer
  dmat<S> ebuff(SF_MAX_ELEM_NODES * row_dpn, SF_MAX_ELEM_NODES * col_dpn);
 
  const vector<mesh_int_t> & petsc_nbr = domain.get_numbering(NBR_PETSC);
 
  // start with assembly
  element_view<mesh_int_t, mesh_real_t> view(domain, NBR_PETSC);
  for(size_t eidx=0; eidx < domain.l_numelem; eidx++)
  {
    // set element view to current element
    view.set_elem(eidx);
 
    // calculate row/col indices of entries
    mesh_int_t nnodes = view.num_nodes();
 
    row_idx.resize(nnodes*row_dpn);
    col_idx.resize(nnodes*col_dpn);
    canonic_indices<mesh_int_t,SF_int>(view.nodes(), petsc_nbr.data(), nnodes, row_dpn, row_idx.data());
    canonic_indices<mesh_int_t,SF_int>(view.nodes(), petsc_nbr.data(), nnodes, col_dpn, col_idx.data());
 
    // call integrator
    integrator(view, ebuff);
 
    // add values into system matrix
    const bool add = true;
    mat.set_values(row_idx, col_idx, ebuff.data(), add);
  }
  // finish assembly and progress output
  mat.finish_assembly();
}
 
template<class T, class S>
inline void assemble_lumped_matrix(abstract_matrix<T,S> & mat,
                                   meshdata<mesh_int_t,mesh_real_t> & domain,
                                   matrix_integrator<mesh_int_t,mesh_real_t> & integrator)
{
  int rank;
  MPI_Comm_rank(domain.comm, &rank);
 
  // we want to make sure that the element integrator fits the matrix
  mesh_int_t row_dpn, col_dpn;
  integrator.dpn(row_dpn, col_dpn);
 
  assert(row_dpn == 1 && row_dpn == mat.dpn_row());
 
  // allocate row / col index buffers
  vector<T> row_idx(SF_MAX_ELEM_NODES * row_dpn);
 
  // allocate elem index buffer
  dmat<S> ebuff(SF_MAX_ELEM_NODES * row_dpn, SF_MAX_ELEM_NODES * col_dpn);
  const vector<mesh_int_t> & petsc_nbr = domain.get_numbering(NBR_PETSC);
 
  // start with assembly
  element_view<mesh_int_t, mesh_real_t> view(domain, NBR_PETSC);
  for(size_t eidx=0; eidx < domain.l_numelem; eidx++)
  {
    // set element view to current element
    view.set_elem(eidx);
 
    // calculate row/col indices of entries
    SF_int nnodes = view.num_nodes();
    row_idx.resize(nnodes*row_dpn);
    canonic_indices<mesh_int_t,T>(view.nodes(), petsc_nbr.data(), nnodes, row_dpn, row_idx.data());
 
    // call integrator
    integrator(view, ebuff);
 
    // do lumping
    for(int i=0; i<nnodes; i++) {
      S rowsum = 0.0;
 
      for(int j=0; j<nnodes; j++)
          rowsum += ebuff[i][j];
 
      // add values into system matrix
      mat.set_value(row_idx[i], row_idx[i], rowsum, true);
    }
  }
  // finish assembly and progress output
  mat.finish_assembly();
}
 
template<class T, class S>
inline void assemble_vector(abstract_vector<T,S> & vec,
                     meshdata<mesh_int_t,mesh_real_t> & domain,
                     vector_integrator<mesh_int_t,mesh_real_t> & integrator)
{
  int rank;
  MPI_Comm_rank(vec.mesh->comm, &rank);
 
  // we want to make sure that the element integrator fits the matrix
  T dpn;
  integrator.dpn(dpn);
  assert(vec.dpn == dpn);
 
  // allocate row / col index buffers
  vector<T> idx(SF_MAX_ELEM_NODES * dpn);
 
  // allocate elem index buffer
  vector<S> ebuff(SF_MAX_ELEM_NODES * dpn);
  const vector<mesh_int_t> & petsc_nbr = domain.get_numbering(NBR_PETSC);
 
  // start with assembly
  element_view<mesh_int_t, mesh_real_t> view(domain, NBR_PETSC);
  for(size_t eidx=0; eidx < domain.l_numelem; eidx++)
  {
    // set element view to current element
    view.set_elem(eidx);
 
    // calculate row/col indices of entries
    SF_int nnodes = view.num_nodes();
    canonic_indices<mesh_int_t,T>(view.nodes(), petsc_nbr.data(), nnodes, dpn, idx.data());
 
    // call integrator
    integrator(view, ebuff.data());
 
    // add values into system matrix
    vec.set(idx, ebuff, ADD_VALUES);
  }
  // finish assembly and progress output
  vec.finish_assembly();
}
 
template<class T, class S> inline
void extract_element_data(const element_view<mesh_int_t,mesh_real_t> & view, abstract_vector<T,S> & vec, SF_real* buffer)
{
  // the dpn has to be set for this to work
  int dpn = vec.dpn;
  typename abstract_vector<T,S>::ltype nodaltype = abstract_vector<T,S>::nodal;
  assert(dpn > 0);
  assert(vec.layout == nodaltype);
 
  for(int i=0; i<view.num_nodes(); i++)
    for(int j=0; j<dpn; j++) {
      int idx = view.node(i)*dpn+j;
      buffer[i*dpn+j] = vec.get(idx);
    }
}
 
template<class T, class S> inline
void set_element_data(const element_view<mesh_int_t,mesh_real_t> & view, SF_real* buffer, abstract_vector<T,S> & vec)
{
  // the dpn has to be set for this to work
  int dpn = vec.dpn;
  typename abstract_vector<T,S>::ltype nodaltype = abstract_vector<T,S>::nodal;
  assert(dpn > 0);
  assert(vec.layout == nodaltype);
 
  SF_real* pvec = vec.ptr();
 
  for(int i=0; i<view.num_nodes(); i++)
    for(int j=0; j<dpn; j++) {
      int idx = view.node(i)*dpn+j;
      pvec[idx] = buffer[i*dpn+j];
    }
 
  vec.release_ptr(pvec);
}
 
template<class T, class S> inline
void get_transformed_pts(const element_view<T,S> & view, Point* loc_pts, Point & trsf_fibre, bool orthogonal=true)
{
  const elem_t type = view.type();
 
  switch(type) {
    case Line: {
      Point p0 = view.coord(0), p1 = view.coord(1);
 
      loc_pts[0] = {0, 0, 0};
      loc_pts[1] = {mag(p1 - p0), 0, 0};
      trsf_fibre = {1, 0, 0};
      break;
    }
 
    case Tri: {
      Point p0 = view.coord(0), p1 = view.coord(1), p2 = view.coord(2);
      Point f = trsf_fibre;
 
      Point p01 = p1 - p0, p02 = p2 - p0;
 
      Point x = normalize(p01);
      Point z = normalize(cross(p01,p02));
      Point y = cross(z, x);
 
      loc_pts[0] = {0, 0, 0};
      loc_pts[1] = {mag(p01), 0, 0};
      loc_pts[2] = {inner_prod(p02, x), inner_prod(p02, y), 0};
      trsf_fibre = {inner_prod(f, x), inner_prod(f, y), 0};
 
      if((fabs(trsf_fibre.x) + fabs(trsf_fibre.y)) < 1e-8 and orthogonal) {
        fprintf(stderr, "Fibre direction is orthogonal to triangle. Assigning (1,0,0) fiber direction.\n");
        trsf_fibre = {1, 0, 0};
      }
      else trsf_fibre = normalize(trsf_fibre);
      break;
    }
 
    case Quad: {
      Point p0 = view.coord(0), p1 = view.coord(1), p2 = view.coord(2), p3 = view.coord(3);
      Point f  = trsf_fibre;
 
      Point p01 = p1 - p0, p02 = p2 - p0, p03 = p3 - p0;
 
      Point x = normalize(p01);
      Point z = normalize(cross(p01, p02));
      Point y = cross(z, x);
 
      loc_pts[0] = {0, 0, 0};
      loc_pts[1] = {mag(p01), 0, 0};
      loc_pts[2] = {inner_prod(p02, x), inner_prod(p02, y), 0};
      loc_pts[3] = {inner_prod(p03, x), inner_prod(p03, y), 0};
 
      trsf_fibre = {inner_prod(f, x), inner_prod(f, y), 0};
 
      if((fabs(trsf_fibre.x) + fabs(trsf_fibre.y)) < 1e-8 and orthogonal) {
        fprintf(stderr, "Fibre direction is orthogonal to quad. Assigning (1,0,0) fiber direction.\n");
        trsf_fibre = {1, 0, 0};
      }
      else trsf_fibre = normalize(trsf_fibre);
      break;
    }
 
    // for 3D elements we just copy over the coords
    default:
      for(int i=0; i<view.num_nodes(); i++)
        loc_pts[i] = view.coord(i);
 
      break;
  }
}
}
 
#endif