core.py

# -*- coding: iso-8859-15 -*-
"""
CALFEM Core module

Contains all the functions implementing CALFEM standard functionality
"""

from scipy.sparse.linalg import dsolve
from scipy.linalg import eig
import numpy as np

import logging as cflog
import sys
import traceback

__prev_exception_hook = sys.excepthook


def exception_logging(exctype, value, tb):
    """
    Log exception by using the root logger.

    Parameters
    ----------
    exctype : type
    value : NameError
    tb : traceback
    """
    write_val = {'exception_type': str(exctype),
                 'message': str(traceback.format_tb(tb, 10))}
    print('Error: %s \n  in "%s", line %d' %
          (value, tb.tb_frame.f_code.co_filename, tb.tb_lineno))


def enable_friendly_errors():
    __prev_exception_hook = sys.excepthook
    sys.excepthook = exception_logging


def disable_friendly_errors():
    sys.excepthook = __prev_exception_hook


easy_on = enable_friendly_errors
easy_off = disable_friendly_errors


def check_list_array(v, error_string):

    fname = sys._getframe(1).f_code.co_name

    if (type(v) != list) and (type(v) != np.ndarray):
        raise TypeError("%s (%s)" % (error_string, fname))


def check_length(v, length, error_string):

    fname = sys._getframe(1).f_code.co_name

    if len(v) != length:
        raise ValueError("%s (%s)" % (error_string, fname))


def user_warning(msg):

    fname = sys._getframe(1).f_code.co_name

    print("Warning: %s (%s)" % (msg, fname))


def error(msg):
    """Write ``msg`` to error log."""
    cflog.error(" calfem.core: "+msg)


def info(msg):
    """Write ``msg`` to info log."""
    cflog.info(" calfem.core: "+msg)


def spring1e(ep):
    """
    Compute element stiffness matrix for spring element.
    
    :param float ep: spring stiffness or analog quantity (ep = k).
    :return mat Ke: stiffness matrix, dim(Ke)= 2 x 2
    """
    k = ep
    return np.mat([[k, -k], [-k, k]], 'd')


def spring1s(ep, ed):
    """
    Compute element force in spring element (spring1e).
    
    :param float ep: spring stiffness or analog quantity
    :param list ed: element displacements [d0, d1]
    :return float es: element force [N]
    """
    k = ep
    return k*(ed[1]-ed[0])


def bar1e(ep):
    """
    Compute element stiffness matrix for spring element.
    
    :param ep float: spring stiffness or analog quantity
    :return mat Ke: stiffness matrix, dim(Ke)= 2 x 2
    """
    k = ep
    return np.mat([[k, -k], [-k, k]], 'd')


def bar1s(ep, ed):
    """
    Compute element force in spring element (spring1e).
    
    :param float ep: spring stiffness or analog quantity
    :param list ed: element displacements [d0, d1]
    :return float es: element force
    """
    k = ep
    return k*(ed[1]-ed[0])


def bar2e(ex, ey, ep):
    """
    Compute the element stiffness matrix for two dimensional bar element.
    
    :param list ex: element x coordinates [x1, x2]
    :param list ey: element y coordinates [y1, y2]
    :param list ep: [E, A]: E - Young's modulus, A - Cross section area
    :return mat Ke: stiffness matrix, [4 x 4]
    """
    E = ep[0]
    A = ep[1]

    b = np.mat([[ex[1]-ex[0]], [ey[1]-ey[0]]])
    L = np.sqrt(b.T*b).item()

    Kle = np.mat([[1., -1.], [-1., 1.]])*E*A/L

    n = np.asarray(b.T/L).reshape(2,)

    G = np.mat([
        [n[0], n[1], 0., 0.],
        [0., 0., n[0], n[1]]
    ])

    return G.T*Kle*G


def bar2g(ex, ey, ep, N):
    """
    Compute element stiffness matrix for two dimensional geometric
    nonlinear bar element.
    
    :param list ex: element x coordinates [x1, x2]
    :param list ey: element y coordinates [y1, y2]
    :param list ep: element properties [E, A], E - Young's modulus, A - Cross section area
    :param float N: normal force
    :return mat Ke: stiffness matrix [4 x 4]
    """
    E = ep[0]
    A = ep[1]

    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]]
    ])

    L = np.sqrt(b.T*b).item()

    n = np.asarray(b.T/L).reshape(2,)

    G = np.mat([
        [n[0], n[1], 0.,   0.],
        [-n[1], n[0], 0.,   0.],
        [0.,   0.,   n[0], n[1]],
        [0.,   0.,  -n[1], n[0]]
    ])

    Kle = E*A/L*np.mat([
        [1, 0, -1, 0],
        [0, 0, 0, 0],
        [-1, 0, 1, 0],
        [0, 0, 0, 0]
    ])+N/L*np.mat([
        [0, 0, 0, 0],
        [0, 1, 0, -1],
        [0, 0, 0, 0],
        [0, -1, 0, 1]
    ])

    return G.T*Kle*G


def bar2s(ex, ey, ep, ed):
    """
    Compute normal force in two dimensional bar element.
    
    :param list ex: element x coordinates [x1, x2]
    :param list ey: element y coordinates [y1, y2]
    :param list ep: element properties [E, A], E - Young's modulus, A - Cross section area
    :param list ed: element displacements [u1, u2, u3, u4]    
    :return float N: element foce [N]    
    """
    E = ep[0]
    A = ep[1]

    b = np.mat([[ex[1]-ex[0]], [ey[1]-ey[0]]])
    L = np.sqrt(b.T*b).item()

    #Kle = np.mat([[1.,-1.],[-1.,1.]])*E*A/L

    n = np.asarray(b.T/L).reshape(2,)

    G = np.mat([
        [n[0], n[1], 0., 0.],
        [0., 0., n[0], n[1]]
    ])

    u = np.asmatrix(ed).T
    N = E*A/L*np.mat([[-1., 1.]])*G*u
    return N.item()


def bar3e(ex, ey, ez, ep):
    """
    Compute element stiffness matrix for three dimensional bar element.
    
    :param list ex: element x coordinates [x1, x2]
    :param list ey: element y coordinates [y1, y2]
    :param list ez: element z coordinates [z1, z2]
    :param list ep: element properties [E, A], E - Young's modulus, A - Cross section area
    :return mat Ke: stiffness matrix, [6 x 6]
    """
    E = ep[0]
    A = ep[1]

    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]],
        [ez[1]-ez[0]]
    ])
    L = np.sqrt(b.T*b).item()

    n = np.asarray(b.T/L).reshape(3)

    G = np.mat([
        [n[0], n[1], n[2], 0.,   0.,   0.],
        [0.,   0.,   0.,   n[0], n[1], n[2]]
    ])

    Kle = E*A/L*np.mat([
        [1, -1],
        [-1, 1]
    ])

    return G.T*Kle*G


def bar3s(ex, ey, ez, ep, ed):
    """
    Compute normal force in three dimensional bar element.
    
    :param list ex: element x coordinates [x1, x2]
    :param list ey: element y coordinates [y1, y2]
    :param list ez: element z coordinates [z1, z2]
    :param list ep: element properties [E, A], E - Young's modulus, A - Cross section area   
    :param list ed: element displacements [u1, ..., u6]
    :return float N: normal force
    """
    E = ep[0]
    A = ep[1]

    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]],
        [ez[1]-ez[0]]
    ])
    L = np.sqrt(b.T*b).item()

    n = np.asarray(b.T/L).reshape(3)

    G = np.mat([
        [n[0], n[1], n[2], 0., 0., 0.],
        [0., 0., 0., n[0], n[1], n[2]]
    ])

    #Kle = E*A/L*np.mat([
    #    [ 1,-1],
    #    [-1, 1]
    #])

    u = np.asmatrix(ed).T
    N = E*A/L*np.mat([[-1., 1.]])*G*u

    return N.item()

def beam1e(ex, ep, eq=None):
    """
    Compute the stiffness matrix for a one dimensional beam element.

    :param list ex: element x coordinates [x1, x2]
    :param list ep: element properties [E, I], E - Young's modulus, I - Moment of inertia
    :param float eq: distributed load [qy]
    :return mat Ke: element stiffness matrix [4 x 4]
    :return mat fe: element stiffness matrix [4 x 1] (if eq!=None)
    """
    L = ex[1]-ex[0]

    E = ep[0]
    I = ep[1]

    qy = 0.
    if not eq is None:
        qy = eq

    Ke = E*I/(L**3) * np.mat([
        [12,    6*L,    -12,    6*L],
        [6*L,   4*L**2, -6*L,   2*L**2],
        [-12,   -6*L,   12,     -6*L],
        [6*L,   2*L**2, -6*L,   4*L**2]
    ])

    fe = qy*np.mat([L/2, L**2/12, L/2, -L**2/12]).T

    if eq is None:
        return Ke
    else:
        return Ke, fe

def beam1s(ex, ep, ed, eq=None, nep=None):
    """
    Compute section forces in one dimensional beam element (beam1e).

    Parameters:

        ex = [x1 x2]        element node coordinates

        ep = [E I]          element properties,
                            E:  Young's modulus
                            I:  moment of inertia

        ed = [u1 ... u4]    element displacements

        eq = qy             distributed load, local directions

        nep                 number of evaluation points ( default=2 )

    Returns:

        es = [ V1 M1        section forces, local directions, in
               V2 M2        n points along the beam, dim(es)= n x 2
               .........]

        edi = [ v1          element displacements, local directions,
                v2          in n points along the beam, dim(es)= n x 1
                .......]

            eci = [ x1      local x-coordinates of the evaluation
                    x2      points, (x1=0 and xn=L)
                    ...]

    """
    EI = ep[0]*ep[1]
    L = ex[1]-ex[0]

    qy = 0.

    if not eq is None:
        qy = eq

    ne = 2

    if nep != None:
        ne = nep

    Cinv = np.mat([
        [1, 0,  0,  0],
        [0, 1,  0,  0],
        [-3/(L**2), -2/L,   3/(L**2),   -1/L],
        [2/(L**3),  1/(L**2),   -2/(L**3),  1/(L**2)]
    ])

    Ca = (Cinv@ed).T

    x = np.asmatrix(np.arange(0., L+L/(ne-1), L/(ne-1))).T
    zero = np.asmatrix(np.zeros([len(x)])).T
    one = np.asmatrix(np.ones([len(x)])).T

    v = np.concatenate((one, x, np.power(x, 2), np.power(x, 3)), 1)@Ca \
                        + qy/(24*EI)*(np.power(x,4) - 2*L*np.power(x,3) + (L**2)*np.power(x,2))
    d2v = np.concatenate((zero, zero, 2*one, 6*x), 1)@Ca \
                        + qy/(2*EI)*(np.power(x,2) - L*x + L**2/12)
    d3v = np.concatenate((zero, zero, zero, 6*one), 1)@Ca - qy*(x - L/2)

    M = EI*d2v
    V = -EI*d3v
    edi = v
    eci = x
    es = np.concatenate((V, M), 1)

    return (es, edi, eci)



def beam2e(ex, ey, ep, eq=None):
    """
    Compute the stiffness matrix for a two dimensional beam element.
    
    :param list ex: element x coordinates [x1, x2]
    :param list ey: element y coordinates [y1, y2]
    :param list ep: element properties [E, A, I], E - Young's modulus, A - Cross section area, I - Moment of inertia   
    :param list eq: distributed loads, local directions [qx, qy]
    :return mat Ke: element stiffness matrix [6 x 6]
    :return mat fe: element stiffness matrix [6 x 1] (if eq!=None)
    """

    b = np.mat([[ex[1]-ex[0]], [ey[1]-ey[0]]])
    L = np.sqrt(b.T*b).item()
    n = np.asarray(b.T/L).reshape(2,)

    E = ep[0]
    A = ep[1]
    I = ep[2]

    qx = 0.
    qy = 0.
    if not eq is None:
        qx = eq[0]
        qy = eq[1]

    Kle = np.mat([
        [E*A/L,      0.,          0.,    -E*A/L,    0.,        0.],
        [0.,    12*E*I/L**3., 6*E*I/L**2.,    0., -12*E*I/L**3., 6*E*I/L**2.],
        [0.,    6*E*I/L**2.,  4*E*I/L,      0., -6*E*I/L**2.,  2*E*I/L],
        [-E*A/L,     0.,          0.,     E*A/L,    0.,        0.],
        [0.,   -12*E*I/L**3., -6*E*I/L**2.,    0.,  12*E*I/L**3., -6*E*I/L**2.],
        [0.,    6*E*I/L**2.,  2*E*I/L,      0.,  -6*E*I/L**2., 4*E*I/L]
    ])

    fle = L*np.mat([qx/2, qy/2, qy*L/12, qx/2, qy/2, -qy*L/12]).T

    G = np.mat([
        [n[0], n[1],  0.,    0.,    0.,   0.],
        [-n[1], n[0],  0.,    0.,    0.,   0.],
        [0.,    0.,    1.,    0.,    0.,   0.],
        [0.,    0.,    0.,   n[0],  n[1],  0.],
        [0.,    0.,    0.,  -n[1],  n[0],  0.],
        [0.,    0.,    0.,    0.,    0.,   1.]
    ])

    Ke = G.T*Kle*G
    fe = G.T*fle

    if eq is None:
        return Ke
    else:
        return Ke, fe


def beam2s(ex, ey, ep, ed, eq=None, nep=None):
    """
    Compute section forces in two dimensional beam element (beam2e).
    
    Parameters:
 
        ex = [x1 x2]
        ey = [y1 y2]        element node coordinates

        ep = [E A I]        element properties,
                            E:  Young's modulus
                            A:  cross section area
                            I:  moment of inertia

        ed = [u1 ... u6]    element displacements

        eq = [qx qy]        distributed loads, local directions 

        nep                 number of evaluation points ( default=2 )
        
    Returns:
          
        es = [ N1 V1 M1     section forces, local directions, in 
               N2 V2 M2     n points along the beam, dim(es)= n x 3
               .........]  
           
        edi = [ u1 v1       element displacements, local directions,
                u2 v2       in n points along the beam, dim(es)= n x 2
                .......]    

            eci = [ x1      local x-coordinates of the evaluation 
                    x2      points, (x1=0 and xn=L)
                    ...]
    
    """
    EA = ep[0]*ep[1]
    EI = ep[0]*ep[2]
    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]]
    ])

    L = np.sqrt(b.T*b).item()
    n = np.asarray(b.T/L).reshape(2,)

    qx = 0.
    qy = 0.

    if not eq is None:
        qx = eq[0]
        qy = eq[1]

    ne = 2

    if nep != None:
        ne = nep

    C = np.mat([
        [0.,   0.,   0.,    1.,   0.,   0.],
        [0.,   0.,   0.,    0.,   0.,   1.],
        [0.,   0.,   0.,    0.,   1.,   0.],
        [L,   0.,   0.,    1.,   0.,   0.],
        [0.,   L**3, L**2,   0.,   L,    1.],
        [0., 3*L**2, 2*L,   0.,   1.,   0.]
    ])

    G = np.mat([
        [n[0], n[1],  0.,    0.,    0.,   0.],
        [-n[1], n[0],  0.,    0.,    0.,   0.],
        [0.,    0.,    1.,    0.,    0.,   0.],
        [0.,    0.,    0.,   n[0],  n[1],  0.],
        [0.,    0.,    0.,  -n[1],  n[0],  0.],
        [0.,    0.,    0.,    0.,    0.,   1.]
    ])

    M = np.ravel(C.I*(G*np.asmatrix(ed).T -
                 np.matrix([0., 0., 0., -qx*L**2/(2*EA), qy*L**4/(24*EI), qy*L**3/(6*EI)]).T))
    A = np.matrix([M[0], M[3]]).T
    B = np.matrix([M[1], M[2], M[4], M[5]]).T

    x = np.asmatrix(np.arange(0., L+L/(ne-1), L/(ne-1))).T
    zero = np.asmatrix(np.zeros([len(x)])).T
    one = np.asmatrix(np.ones([len(x)])).T

    u = np.concatenate((x, one), 1)*A-np.power(x, 2)*qx/(2*EA)
    du = np.concatenate((one, zero), 1)*A-x*qx/EA
    v = np.concatenate((np.power(x, 3), np.power(x, 2), x,
                       one), 1)*B+np.power(x, 4)*qy/(24*EI)
    d2v = np.concatenate((6*x, 2*one, zero, zero), 1) * \
        B+np.power(x, 2)*qy/(2*EI)
    d3v = np.concatenate((6*one, zero, zero, zero), 1)*B+x*qy/EI

    N = EA*du
    M = EI*d2v
    V = -EI*d3v
    edi = np.concatenate((u, v), 1)
    eci = x
    es = np.concatenate((N, V, M), 1)

    return (es, edi, eci)


def beam2t(ex, ey, ep, eq=None):
    """
    Compute the stiffness matrix for a two dimensional elastic
    Timoshenko beam element.
    
    Parameters:
     
        ex = [x1 x2]
        ey = [y1 y2]        element node coordinates
    
        ep = [E G A I ks]   element properties
                              E: Young's modulus
                              G: Shear modulus
                              A: Cross section area
                              I: Moment of inertia
                             ks: Shear correction factor
    
        eq = [qx qy]        distributed loads, local directions
        
    Returns:
     
        Ke                  element stiffness matrix (6 x 6)
    
        fe                  element load vector (6 x 1)
    
    """

    b = np.mat([[ex[1]-ex[0]], [ey[1]-ey[0]]])
    L = np.sqrt(b.T*b).item()
    n = np.asarray(b.T/L).reshape(2)

    E = ep[0]
    Gm = ep[1]
    A = ep[2]
    I = ep[3]
    ks = ep[4]

    qx = 0.
    qy = 0.
    if eq != None:
        qx = eq[0]
        qy = eq[1]

    m = (12/L**2)*(E*I/(Gm*A*ks))

    Kle = E/(1+m)*np.mat([
        [A*(1+m)/L,      0.,         0.,        -A*(1+m)/L,     0.,          0.],
        [0.,         12*I/L**3., 6*I/L**2.,         0.,    -12*I/L**3., 6*I/L**2.],
        [0.,         6*I/L**2.,  4*I *
            (1+m/4.)/L,    0.,    -6*I/L**2.,  2*I*(1-m/2)/L],
        [-A*(1+m)/L,     0.,         0.,
         A*(1+m)/L,     0.,          0.],
        [0.,        -12*I/L**3., -6*I/L**2.,
            0.,     12*I/L**3., -6*I/L**2.],
        [0.,         6*I/L**2.,  2*I *
            (1-m/2)/L,     0.,    -6*I/L**2.,  4*I*(1+m/4)/L]
    ])

    fle = L*np.mat([qx/2, qy/2, qy*L/12, qx/2, qy/2, -qy*L/12]).T

    G = np.mat([
        [n[0], n[1],  0.,   0.,   0.,   0.],
        [-n[1], n[0],  0.,   0.,   0.,   0.],
        [0.,   0.,   1.,   0.,   0.,   0.],
        [0.,   0.,   0.,  n[0], n[1],  0.],
        [0.,   0.,   0., -n[1], n[0],  0.],
        [0.,   0.,   0.,   0.,   0.,   1.]
    ])

    Ke = G.T*Kle*G
    fe = G.T*fle

    if eq is None:
        return Ke
    else:
        return Ke, fe


def beam2ts(ex, ey, ep, ed, eq=None, nep=None):
    """
    Compute section forces in two dimensional beam element (beam2e).
    
    Parameters:
 
        ex = [x1, x2]
        ey = [y1, y2]       element node coordinates

        ep = [E,G,A,I,ks]   element properties,
                              E:  Young's modulus
                              G:  shear modulus
                              A:  cross section area
                              I:  moment of inertia

        ed = [u1, ... ,u6]  element displacements

        eq = [qx, qy]       distributed loads, local directions 

        nep                 number of evaluation points ( default=2 )
        
    Returns:
          
        es = [[N1,V1,M1],   section forces, local directions, in 
              [N2,V2,M2],   n points along the beam, dim(es)= n x 3
              ..........]  
    
        edi = [[u1,v1,teta1],   element displacements, local directions,
               [u2,v2,teta2],   and rotation of cross section at
               .............]   in n points along the beam, dim(es)= n x 2
    
    (Note! Rotation of the cross section is not equal to dv/dx for Timoshenko beam element)
    
        eci = [[x1],    local x-coordinates of the evaluation 
               [x2],    points, (x1=0 and xn=L)
               ....]
    
    """
    EA = ep[0]*ep[2]
    EI = ep[0]*ep[3]
    GAK = ep[1]*ep[2]*ep[4]
    alfa = EI/GAK

    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]]
    ])
    L = np.sqrt(b.T*b).item()
    n = np.asarray(b.T/L).reshape(2)

    qx = 0.
    qy = 0.
    if eq != None:
        qx = eq[0]
        qy = eq[1]

    ne = 2

    if nep != None:
        ne = nep

    C = np.mat([
        [0., 0.,              0.,   1., 0., 0.],
        [0., 0.,              0.,   0., 0., 1.],
        [0., 6*alfa,          0.,   0., 1., 0.],
        [L,  0.,              0.,   1., 0., 0.],
        [0., L**3,            L**2, 0., L,  1.],
        [0., 3*(L**2+2*alfa), 2*L,  0., 1., 0.]
    ])

    G = np.mat([
        [n[0], n[1], 0., 0.,   0.,   0.],
        [-n[1], n[0], 0., 0.,   0.,   0.],
        [0.,   0.,   1., 0.,   0.,   0.],
        [0.,   0.,   0., n[0], n[1], 0.],
        [0.,   0.,   0., -n[1], n[0], 0.],
        [0.,   0.,   0., 0.,   0.,   1.]
    ])

    M = np.ravel(C.I*(G*np.asmatrix(ed).T-np.mat([0., 0., 0., -qx*L**2/(
        2*EA), qy*L**4/(24*EI)-qy*L**2/(2*GAK), qy*L**3/(6*EI)]).T))
    C2 = np.mat([M[0], M[3]]).T
    C4 = np.mat([M[1], M[2], M[4], M[5]]).T

    x = np.asmatrix(np.arange(0., L+L/(ne-1), L/(ne-1))).T
    zero = np.asmatrix(np.zeros([len(x)])).T
    one = np.asmatrix(np.ones([len(x)])).T

    u = np.concatenate((x, one), 1)*C2-qx/(2*EA)*np.power(x, 2)
    du = np.concatenate((one, zero), 1)*C2-qx*x/EA

    v = np.concatenate((np.power(x, 3), np.power(x, 2), x, one), 1) * \
        C4+qy/(24*EI)*np.np.power(x, 4)-qy/(2*GAK)*np.power(x, 2)
    dv = np.concatenate((3*np.power(x, 2), 2*x, one, zero),
                        1)*C4+qy*np.power(x, 3)/(6*EI)-qy*x/GAK

    teta = np.concatenate((3*(np.power(x, 2)+2*alfa*one),
                          2*x, one, zero), 1)*C4+qy*np.power(x, 3)/(6*EI)
    dteta = np.concatenate((6*x, 2*one, zero, zero), 1) * \
        C4+qy*np.power(x, 2)/(2*EI)

    N = EA*du
    M = EI*dteta
    V = GAK*(dv-teta)

    es = np.concatenate((N, V, M), 1)
    edi = np.concatenate((u, v, teta), 1)
    eci = x

    if nep != None:
        return es, edi, eci
    else:
        return es


def beam2w(ex, ey, ep, eq=None):
    """
    Compute the stiffness matrix for a two dimensional beam element
    on elastic foundation.
    
    Parameters:
 
        ex = [x1, x2]
        ey = [y1, y2]       element node coordinates

        ep = [E,A,I,ka,kt]  element properties,
                              E:  Young's modulus
                              A:  cross section area
                              I:  moment of inertia
                             ka:  axial foundation stiffness
                             kt:  transversal foundation stiffness

        eq = [qx, qy]       distributed loads, local directions

    Returns:

        Ke                  beam stiffness matrix (6 x 6)
        
        fe                  element load vector (6 x 1)
    """
    b = np.mat([[ex[1]-ex[0]], [ey[1]-ey[0]]])
    L = np.sqrt(b.T*b).item()
    n = np.asarray(b/L).reshape(2)

    E, A, I, ka, kt = ep

    qx = 0
    qy = 0
    if eq != None:
        qx, qy = eq

    K1 = np.mat([
        [E*A/L,  0,           0,         -E*A/L, 0,           0],
        [0,      12*E*I/L**3, 6*E*I/L**2, 0,    -12*E*I/L**3, 6*E*I/L**2],
        [0,      6*E*I/L**2,  4*E*I/L,    0,    -6*E*I/L**2,  2*E*I/L],
        [-E*A/L,  0,           0,          E*A/L, 0,           0],
        [0,     -12*E*I/L**3, -6*E*I/L**2, 0,     12*E*I/L**3, -6*E*I/L**2],
        [0,      6*E*I/L**2,  2*E*I/L,    0,    -6*E*I/L**2,  4*E*I/L]
    ])

    K2 = L/420*np.mat([
        [140*ka, 0,       0,         70*ka,  0,       0],
        [0,      156*kt,  22*kt*L,   0,      54*kt,  -13*kt*L],
        [0,      22*kt*L, 4*kt*L**2, 0,      13*kt*L, -3*kt*L**2],
        [70*ka,  0,       0,         140*ka, 0,       0],
        [0,      54*kt,   13*kt*L,   0,      156*kt, -22*kt*L],
        [0,     -13*kt*L, -3*kt*L**2, 0,     -22*kt*L, 4*kt*L**2]
    ])

    Kle = K1+K2
    fle = L*np.mat([qx/2, qy/2, qy*L/12, qx/2, qy/2, -qy*L/12]).T

    G = np.mat([
        [n[0], n[1], 0, 0,    0,    0],
        [-n[1], n[0], 0, 0,    0,    0],
        [0,    0,    1, 0,    0,    0],
        [0,    0,    0, n[0], n[1], 0],
        [0,    0,    0, -n[1], n[0], 0],
        [0,    0,    0, 0,    0,    1]
    ])

    Ke = G.T*Kle*G
    fe = G.T*fle

    if eq != None:
        return Ke, fe
    else:
        return Ke


def beam2ws(ex, ey, ep, ed, eq=None):
    """
    Compute section forces in a two dimensional beam element
    on elastic foundation.
    
    Parameters:
 
        ex = [x1, x2]
        ey = [y1, y2]           element node coordinates

        ep = [E,A,I,ka,kt]      element properties,
                                  E:  Young's modulus
                                  A:  cross section area
                                  I:  moment of inertia
                                 ka:  axial foundation stiffness
                                 kt:  transversal foundation stiffness

        ed = [u1, ... ,u6]      element displacement vector

        eq = [qx, qy]           distributed loads, local directions

    Returns:

        es = [[N1, V1, M1],
              [N2, V2, M2]]     element forces, local direction
    """
    if np.asmatrix(ed).shape[0] > 1:
        error("Only one row is allowed in the ed matrix !!!")
        return

    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]]
    ])
    L = np.sqrt(b.T*b).item()
    n = np.asarray(b/L).reshape(2,)

    E, A, I, ka, kt = ep

    qx = 0
    qy = 0
    if eq != None:
        qx, qy = eq

    K1 = np.mat([
        [E*A/L, 0,           0,         -E*A/L, 0,           0],
        [0,     12*E*I/L**3, 6*E*I/L**2, 0,    -12*E*I/L**3, 6*E*I/L**2],
        [0,     6*E*I/L**2,  4*E*I/L,    0,    -6*E*I/L**2,  2*E*I/L],
        [-E*A/L, 0,           0,          E*A/L, 0,           0],
        [0,    -12*E*I/L**3, -6*E*I/L**2, 0,     12*E*I/L**3, -6*E*I/L**2],
        [0,     6*E*I/L**2,  2*E*I/L,    0,    -6*E*I/L**2,  4*E*I/L]
    ])

    K2 = L/420*np.mat([
        [140*ka, 0,       0,         70*ka,  0,       0],
        [0,      156*kt,  22*kt*L,   0,      54*kt,  -13*kt*L],
        [0,      22*kt*L, 4*kt*L**2, 0,      13*kt*L, -3*kt*L**2],
        [70*ka,  0,       0,         140*ka, 0,       0],
        [0,      54*kt,   13*kt*L,   0,      156*kt, -22*kt*L],
        [0,     -13*kt*L, -3*kt*L**2, 0,     -22*kt*L, 4*kt*L**2]
    ])

    Kle = K1+K2
    fle = L*np.mat([qx/2, qy/2, qy*L/12, qx/2, qy/2, -qy*L/12]).T

    G = np.mat([
        [n[0], n[1], 0, 0,    0,    0],
        [-n[1], n[0], 0, 0,    0,    0],
        [0,    0,    1, 0,    0,    0],
        [0,    0,    0, n[0], n[1], 0],
        [0,    0,    0, -n[1], n[0], 0],
        [0,    0,    0, 0,    0,    1]
    ])

    P = Kle*G*np.asmatrix(ed).T-fle

    es = np.mat([
        [-P[0, 0], -P[1, 0], -P[2, 0]],
        [P[3, 0], P[4, 0], P[5, 0]]
    ])

    return es


def beam2g(ex, ey, ep, N, eq=None):
    """
    Compute the element stiffness matrix for a two dimensional
    beam element with respect to geometric nonlinearity.
    
    Parameters:
 
        ex = [x1, x2]
        ey = [y1, y2]           element node coordinates

        ep = [E,A,I]            element properties;
                                  E:  Young's modulus
                                  A:  cross section area
                                  I:  moment of inertia

        N                       axial force in the beam

        eq                      distributed transverse load

    Returns:

        Ke                      element stiffness matrix (6 x 6)
        
        fe                      element load vector (6 x 1)
    """
    if eq != None:
        if np.size(eq) > 1:
            error("eq should be a scalar !!!")
            return
        else:
            q = eq[0]
    else:
        q = 0

    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]]
    ])
    L = np.sqrt(b.T*b).item()
    n = np.asarray(b/L).reshape(2,)

    E, A, I = ep

    rho = -N*L**2/(np.pi**2*E*I)

    kL = np.pi*np.sqrt(abs(rho))+np.finfo(float).eps

    if rho > 0:
        f1 = (kL/2)/np.tan(kL/2)
        f2 = (1/12.)*kL**2/(1-f1)
        f3 = f1/4+3*f2/4
        f4 = -f1/2+3*f2/2
        f5 = f1*f2
        h = 6*(2/kL**2-(1+np.cos(kL))/(kL*np.sin(kL)))
    elif rho < 0:
        f1 = (kL/2)/np.tanh(kL/2)
        f2 = -(1/12.)*kL**2/(1-f1)
        f3 = f1/4+3*f2/4
        f4 = -f1/2+3*f2/2
        f5 = f1*f2
        h = -6*(2/kL**2-(1+np.cosh(kL))/(kL*np.sinh(kL)))
    else:
        f1 = f2 = f3 = f4 = f5 = h = 1

    Kle = np.mat([
        [E*A/L, 0.,              0.,            -E*A/L, 0.,              0.],
        [0.,    12*E*I*f5/L**3., 6*E*I*f2/L**2.,
            0.,   -12*E*I*f5/L**3., 6*E*I*f2/L**2.],
        [0.,    6*E*I*f2/L**2.,  4*E*I*f3/L,
            0.,   -6*E*I*f2/L**2.,  2*E*I*f4/L],
        [-E*A/L, 0.,              0.,             E*A/L, 0.,              0.],
        [0.,   -12*E*I*f5/L**3., -6*E*I*f2/L**2.,
            0.,    12*E*I*f5/L**3., -6*E*I*f2/L**2.],
        [0.,    6*E*I*f2/L**2.,  2*E*I*f4/L,
            0.,   -6*E*I*f2/L**2.,  4*E*I*f3/L]
    ])

    fle = q*L*np.mat([0., 1/2., L*h/12, 0., 1/2., -L*h/12]).T

    G = np.mat([
        [n[0], n[1], 0, 0,    0,    0],
        [-n[1], n[0], 0, 0,    0,    0],
        [0,    0,    1, 0,    0,    0],
        [0,    0,    0, n[0], n[1], 0],
        [0,    0,    0, -n[1], n[0], 0],
        [0,    0,    0, 0,    0,    1]
    ])

    Ke = G.T*Kle*G
    fe = G.T*fle

    if eq != None:
        return Ke, fe
    else:
        return Ke


def beam2gs(ex, ey, ep, ed, N, eq=None):
    """
    Calculate section forces in a two dimensional nonlinear
    beam element.

    Parameters:
 
        ex = [x1, x2]
        ey = [y1, y2]           element node coordinates

        ep = [E,A,I]            element properties;
                                  E:  Young's modulus
                                  A:  cross section area
                                  I:  moment of inertia

        ed = [u1, ... ,u6]      element displacement vector

        N                       axial force

        eq = [qy]               distributed transverse load

    Returns:

        es = [[N1,V1,M1],       element forces, local directions
              [N2,V2,M2]]
    """
    if eq != None:
        eq = eq[0]
    else:
        eq = 0

    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]]
    ])
    L = np.sqrt(b.T*b).item()
    n = np.asarray(b/L).reshape(2,)

    E, A, I = ep

    rho = -N*L**2/(np.pi**2*E*I)

    eps = 2.2204e-16
    kL = np.pi*np.sqrt(abs(rho))+eps

    if rho > 0:
        f1 = (kL/2)/np.tan(kL/2)
        f2 = (1/12.)*kL**2/(1-f1)
        f3 = f1/4+3*f2/4
        f4 = -f1/2+3*f2/2
        f5 = f1*f2
        h = 6*(2/kL**2-(1+np.cos(kL))/(kL*np.sin(kL)))
    elif rho < 0:
        f1 = (kL/2)/np.tanh(kL/2)
        f2 = -(1/12.)*kL**2/(1-f1)
        f3 = f1/4+3*f2/4
        f4 = -f1/2+3*f2/2
        f5 = f1*f2
        h = -6*(2/kL**2-(1+np.cosh(kL))/(kL*np.sinh(kL)))
    else:
        f1 = f2 = f3 = f4 = f5 = h = 1

    Kle = np.mat([
        [E*A/L, 0,              0,            -E*A/L, 0,              0],
        [0,     12*E*I*f5/L**3, 6*E*I*f2/L**2,
            0,    -12*E*I*f5/L**3, 6*E*I*f2/L**2],
        [0,     6*E*I*f2/L**2,  4*E*I*f3/L,    0,    -6*E*I*f2/L**2,  2*E*I*f4/L],
        [-E*A/L, 0,              0,             E*A/L, 0,              0],
        [0,    -12*E*I*f5/L**3, -6*E*I*f2/L**2,
            0,     12*E*I*f5/L**3, -6*E*I*f2/L**2],
        [0,     6*E*I*f2/L**2,  2*E*I*f4/L,    0,    -6*E*I*f2/L**2,  4*E*I*f3/L]
    ])

    fle = eq*L*np.mat([0, 1/2., L*h/12, 0, 1/2., -L*h/12]).T

    G = np.mat([
        [n[0], n[1], 0, 0,    0,    0],
        [-n[1], n[0], 0, 0,    0,    0],
        [0,    0,    1, 0,    0,    0],
        [0,    0,    0, n[0], n[1], 0],
        [0,    0,    0, -n[1], n[0], 0],
        [0,    0,    0, 0,    0,    1]
    ])

    u = np.asmatrix(ed).T
    P = Kle*G*u-fle

    es = np.mat([
        [-P[0, 0], -P[1, 0], -P[2, 0]],
        [P[3, 0], P[4, 0], P[5, 0]]
    ])

    return es


def beam2d(ex, ey, ep):
    """
    Calculate the stiffness matrix Ke, the mass matrix Me
    and the damping matrix Ce for a 2D elastic Bernoulli
    beam element.

    Parameters:
 
        ex = [x1, x2]
        ey = [y1, y2]           element node coordinates

        ep = [E,A,I,m,(a,b)]    element properties;
                                  E:  Young's modulus
                                  A:  cross section area
                                  I:  moment of inertia
                                  m:  mass per unit length
                                a,b:  damping coefficients,
                                      Ce=aMe+bKe

    Returns:

        Ke                      element stiffness matrix (6 x 6)
        Me                      element mass martix
        Ce                      element damping matrix, optional
    """
    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]]
    ])
    L = np.sqrt(b.T*b).item()
    n = np.asarray(b/L).reshape(2,)

    a = 0
    b = 0
    if np.size(ep) == 4:
        E, A, I, m = ep
    elif np.size(ep) == 6:
        E, A, I, m, a, b = ep

    Kle = np.mat([
        [E*A/L, 0,           0,         -E*A/L, 0,           0],
        [0,     12*E*I/L**3, 6*E*I/L**2, 0,    -12*E*I/L**3, 6*E*I/L**2],
        [0,     6*E*I/L**2,  4*E*I/L,    0,    -6*E*I/L**2,  2*E*I/L],
        [-E*A/L, 0,           0,          E*A/L, 0,           0],
        [0,    -12*E*I/L**3, -6*E*I/L**2, 0,     12*E*I/L**3, -6*E*I/L**2],
        [0,     6*E*I/L**2,  2*E*I/L,    0,    -6*E*I/L**2,  4*E*I/L]
    ])

    Mle = m*L/420*np.mat([
        [140, 0,    0,      70,  0,    0],
        [0,   156,  22*L,   0,   54,  -13*L],
        [0,   22*L, 4*L**2, 0,   13*L, -3*L**2],
        [70,  0,    0,      140, 0,    0],
        [0,   54,   13*L,   0,   156, -22*L],
        [0,  -13*L, -3*L**2, 0,  -22*L, 4*L**2]
    ])

    Cle = a*Mle+b*Kle

    G = np.mat([
        [n[0], n[1], 0, 0,    0,    0],
        [-n[1], n[0], 0, 0,    0,    0],
        [0,    0,    1, 0,    0,    0],
        [0,    0,    0, n[0], n[1], 0],
        [0,    0,    0, -n[1], n[0], 0],
        [0,    0,    0, 0,    0,    1]
    ])

    Ke = G.T*Kle*G
    Me = G.T*Mle*G
    Ce = G.T*Cle*G

    if np.size(ep) == 4:
        return Ke, Me
    elif np.size(ep) == 6:
        return Ke, Me, Ce


def beam3e(ex, ey, ez, eo, ep, eq=None):
    """
    Calculate the stiffness matrix for a 3D elastic Bernoulli
    beam element.
    
    Parameters:
     
        ex = [x1 x2]
        ey = [y1 y2]
        ez = [z1 z2]            element node coordinates
        
        eo = [xz yz zz]         orientation of local z axis
        
        ep = [E G A Iy Iz Kv]   element properties
                                  E: Young's modulus
                                  G: Shear modulus
                                  A: Cross section area
                                 Iy: Moment of inertia, local y-axis
                                 Iz: Moment of inertia, local z-axis
                                 Kv: Saint-Venant's torsion constant
    
        eq = [qx qy qz qw]      distributed loads

    Returns:

        Ke                      beam stiffness matrix (12 x 12)

        fe                      equivalent nodal forces (12 x 1)

    """
    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]],
        [ez[1]-ez[0]]
    ])
    L = np.sqrt(b.T*b).item()
    n1 = np.asarray(b.T/L).reshape(3,)

    eo = np.asmatrix(eo)
    lc = np.sqrt(eo*eo.T).item()
    n3 = np.asarray(eo/lc).reshape(3,)

    E, Gs, A, Iy, Iz, Kv = ep

    qx = 0.
    qy = 0.
    qz = 0.
    qw = 0.
    if eq != None:
        qx, qy, qz, qw = eq

    a = E*A/L
    b = 12*E*Iz/L**3
    c = 6*E*Iz/L**2
    d = 12*E*Iy/L**3
    e = 6*E*Iy/L**2
    f = Gs*Kv/L
    g = 2*E*Iy/L
    h = 2*E*Iz/L

    Kle = np.mat([
        [a, 0, 0, 0, 0,   0,  -a, 0, 0, 0, 0,   0],
        [0, b, 0, 0, 0,   c,   0, -b, 0, 0, 0,   c],
        [0, 0, d, 0, -e,   0,   0, 0, -d, 0, -e,   0],
        [0, 0, 0, f, 0,   0,   0, 0, 0, -f, 0,   0],
        [0, 0, -e, 0, 2*g, 0,   0, 0, e, 0, g,   0],
        [0, c, 0, 0, 0,   2*h, 0, -c, 0, 0, 0,   h],
        [-a, 0, 0, 0, 0,   0,   a, 0, 0, 0, 0,   0],
        [0, -b, 0, 0, 0,  -c,   0, b, 0, 0, 0,  -c],
        [0, 0, -d, 0, e,   0,   0, 0, d, 0, e,   0],
        [0, 0, 0, -f, 0,   0,   0, 0, 0, f, 0,   0],
        [0, 0, -e, 0, g,   0,   0, 0, e, 0, 2*g, 0],
        [0, c, 0, 0, 0,   h,   0, -c, 0, 0, 0,   2*h]
    ])

    fle = L/2*np.mat([qx, qy, qz, qw, -qz*L/6, qy*L/6,
                     qx, qy, qz, qw, qz*L/6, -qy*L/6]).T

    n2 = np.array([0., 0., 0.])
    n2[0] = n3[1]*n1[2]-n3[2]*n1[1]
    n2[1] = -n1[2]*n3[0]+n1[0]*n3[2]
    n2[2] = n3[0]*n1[1]-n1[0]*n3[1]

    #An = np.append([n1,n2],[n3],0)

    G = np.mat([
        [n1[0], n1[1], n1[2], 0,     0,     0,
            0,     0,     0,     0,     0,     0],
        [n2[0], n2[1], n2[2], 0,     0,     0,
            0,     0,     0,     0,     0,     0],
        [n3[0], n3[1], n3[2], 0,     0,     0,
            0,     0,     0,     0,     0,     0],
        [0,     0,     0,     n1[0], n1[1], n1[2],
            0,     0,     0,     0,     0,     0],
        [0,     0,     0,     n2[0], n2[1], n2[2],
            0,     0,     0,     0,     0,     0],
        [0,     0,     0,     n3[0], n3[1], n3[2],
            0,     0,     0,     0,     0,     0],
        [0,     0,     0,     0,     0,     0,
            n1[0], n1[1], n1[2], 0,     0,     0],
        [0,     0,     0,     0,     0,     0,
            n2[0], n2[1], n2[2], 0,     0,     0],
        [0,     0,     0,     0,     0,     0,
            n3[0], n3[1], n3[2], 0,     0,     0],
        [0,     0,     0,     0,     0,     0,     0,
            0,     0,     n1[0], n1[1], n1[2]],
        [0,     0,     0,     0,     0,     0,     0,
            0,     0,     n2[0], n2[1], n2[2]],
        [0,     0,     0,     0,     0,     0,
            0,     0,     0,     n3[0], n3[1], n3[2]]
    ])

    Ke = G.T*Kle*G
    fe = G.T*fle

    if eq is None:
        return Ke
    else:
        return Ke, fe


def beam3s(ex, ey, ez, eo, ep, ed, eq=None, n=None):
    """
    Calculate the variation of the section forces and displacements
    along a three-dimensional beam element.
    
    Parameters:
     
        ex = [x1 x2]                element node coordinates
        ey = [y1 y2]
        ez = [z1 z2]

        eo = [xz yz zz]             orientation of local z axis
        
        ep = [E G A Iy Iz Kv]       element properties
                                      E: Young's modulus
                                      G: Shear modulus
                                      A: Cross section area
                                     Iy: Moment of inertia, local y-axis
                                     Iz: Moment of inertia, local z-axis
                                     Kv: Saint-Venant's torsion constant

        ed                          the element displacement vector from the
                                    global coordinate system
    
        eq = [qx qy qz qw]          the disibuted axial, transversal and
                                    torsional loads

        n                           the number of point in which displacements
                                    and section forces are to be computed

    Returns:

        es = [[N1,Vy1,Vz1,T1,My1,Mz1],  section forces in n points along
              [N2,Vy2,Vz2,T2,My2,Mz2],  the local x-axis
              [..,...,...,..,...,...],
              [Nn,Vyn,Vzn,Tn,Myn,Mzn]]

        edi = [[u1,v1,w1,fi1],          displacements in n points along
               [u2,v2,w2,fi2],          the local x-axis
               [..,..,..,...],
               [un,vn,wn,fin]]

        eci = [[x1],                    local x-coordinates of the evaluation
               [x2],                    points
               [..],
               [xn]]

    """
    b = np.mat([
        [ex[1]-ex[0]],
        [ey[1]-ey[0]],
        [ez[1]-ez[0]]
    ])
    L = np.sqrt(b.T*b).item()
    n1 = np.asarray(b.T/L).reshape(3,)

    eo = np.asmatrix(eo)
    lc = np.sqrt(eo*eo.T).item()
    n3 = np.asarray(eo/lc).reshape(3,)

    EA = ep[0]*ep[2]
    EIy = ep[0]*ep[3]
    EIz = ep[0]*ep[4]
    GKv = ep[1]*ep[5]

    qx = 0.
    qy = 0.
    qz = 0.
    qw = 0.
    if eq != None:
        qx, qy, qz, qw = eq

    ne = 2
    if n != None:
        ne = n

    n2 = np.array([0., 0., 0.])
    n2[0] = n3[1]*n1[2]-n3[2]*n1[1]
    n2[1] = -n1[2]*n3[0]+n1[0]*n3[2]
    n2[2] = n3[0]*n1[1]-n1[0]*n3[1]

    G = np.mat([
        [n1[0], n1[1], n1[2], 0,     0,     0,
            0,     0,     0,     0,     0,     0],
        [n2[0], n2[1], n2[2], 0,     0,     0,
            0,     0,     0,     0,     0,     0],
        [n3[0], n3[1], n3[2], 0,     0,     0,
            0,     0,     0,     0,     0,     0],
        [0,     0,     0,     n1[0], n1[1], n1[2],
            0,     0,     0,     0,     0,     0],
        [0,     0,     0,     n2[0], n2[1], n2[2],
            0,     0,     0,     0,     0,     0],
        [0,     0,     0,     n3[0], n3[1], n3[2],
            0,     0,     0,     0,     0,     0],
        [0,     0,     0,     0,     0,     0,
            n1[0], n1[1], n1[2], 0,     0,     0],
        [0,     0,     0,     0,     0,     0,
            n2[0], n2[1], n2[2], 0,     0,     0],
        [0,     0,     0,     0,     0,     0,
            n3[0], n3[1], n3[2], 0,     0,     0],
        [0,     0,     0,     0,     0,     0,     0,
            0,     0,     n1[0], n1[1], n1[2]],
        [0,     0,     0,     0,     0,     0,     0,
            0,     0,     n2[0], n2[1], n2[2]],
        [0,     0,     0,     0,     0,     0,
            0,     0,     0,     n3[0], n3[1], n3[2]]
    ])

    u = G*np.asmatrix(ed).T-np.array([    # u is the local element displacement
        [0],        # vector minus the particular solution
        [0],        # to the beam's diff.eq:s
        [0],
        [0],
        [0],
        [0],
        [-qx*L**2/(2*EA)],
        [qy*L**4/(24*EIz)],
        [qz*L**4/(24*EIy)],
        [-qw*L**2/(2*GKv)],
        [-qz*L**3/(6*EIy)],
        [qy*L**3/(6*EIz)]
    ])

    C = np.mat([
        [0, 1, 0,      0,    0, 0, 0,      0,    0, 0, 0, 0],
        [0, 0, 0,      0,    0, 1, 0,      0,    0, 0, 0, 0],
        [0, 0, 0,      0,    0, 0, 0,      0,    0, 1, 0, 0],
        [0, 0, 0,      0,    0, 0, 0,      0,    0, 0, 0, 1],
        [0, 0, 0,      0,    0, 0, 0,      0,   -1, 0, 0, 0],
        [0, 0, 0,      0,    1, 0, 0,      0,    0, 0, 0, 0],
        [L, 1, 0,      0,    0, 0, 0,      0,    0, 0, 0, 0, ],
        [0, 0, L**3,   L**2, L, 1, 0,      0,    0, 0, 0, 0],
        [0, 0, 0,      0,    0, 0, L**3,   L**2, L, 1, 0, 0],
        [0, 0, 0,      0,    0, 0, 0,      0,    0, 0, L, 1],
        [0, 0, 0,      0,    0, 0, -3*L**2, -2*L, -1, 0, 0, 0],
        [0, 0, 3*L**2, 2*L,  1, 0, 0,      0,    0, 0, 0, 0],
    ])

    m = np.linalg.inv(C)*u
    eci = np.zeros((ne, 1))
    es = np.zeros((ne, 6))
    edi = np.zeros((ne, 4))
    for i in np.arange(ne):
        x = i*L/(ne-1)
        eci[i, 0] = x
        es[i, :] = (np.mat([
            [EA, 0, 0,       0,     0, 0, 0,       0,     0, 0, 0,   0],
            [0,  0, -6*EIz,   0,     0, 0, 0,       0,     0, 0, 0,   0],
            [0,  0, 0,       0,     0, 0, -6*EIy,   0,     0, 0, 0,   0],
            [0,  0, 0,       0,     0, 0, 0,       0,     0, 0, GKv, 0],
            [0,  0, 0,       0,     0, 0, -6*EIy*x, -2*EIy, 0, 0, 0,   0],
            [0,  0, 6*EIz*x, 2*EIz, 0, 0, 0,       0,     0, 0, 0,   0]
        ])*m+np.array([-qx*x, -qy*x, -qz*x, -qw*x, -qz*x**2/2, qy*x**2/2]).reshape(6, 1)).T

        edi[i, :] = (np.mat([
            [x, 1, 0,    0,    0, 0, 0,    0,    0, 0, 0, 0],
            [0, 0, x**3, x**2, x, 1, 0,    0,    0, 0, 0, 0],
            [0, 0, 0,    0,    0, 0, x**3, x**2, x, 1, 0, 0],
            [0, 0, 0,    0,    0, 0, 0,    0,    0, 0, x, 1]
        ])*m+np.array([-qx*x**2/(2*EA), qy*x**4/(24*EIz), qz*x**4/(24*EIy), -qw*x**2/(2*GKv)]).reshape(4, 1)).T

    if n is None:
        return es
    else:
        return es, edi, eci


def flw2te(ex, ey, ep, D, eq=None):
    """
    Compute element stiffness (conductivity) matrix for a triangular field element.
    
    Parameters:
    
        ex = [x1 x2 x3]
        ey = [y1 y2 y3]     element coordinates
    
        ep = [t]            element thickness    

        D = [kxx kxy;
             kyx kyy]       constitutive matrix
    
             eq             heat supply per unit volume
             
    Returns:
    
        Ke                  element 'stiffness' matrix (3 x 3)

        fe                  element load vector (3 x 1)
    
    """
    t = ep[0]
    if eq is None:
        eq = 0.

    exm = np.asmatrix(ex)
    eym = np.asmatrix(ey)
    C = np.asmatrix(np.hstack([np.ones((3, 1)), exm.T, eym.T]))
    B = np.matrix([
        [0., 1., 0.],
        [0., 0., 1.]
    ])*C.I
    A = 0.5*np.linalg.det(C)

    Ke = B.T*D*B*t*A
    fe = np.matrix([[1., 1., 1.]]).T*eq*A*t/3

    if eq == 0.:
        return Ke
    else:
        return Ke, fe


def flw2ts(ex, ey, D, ed):
    """
    Compute flows or corresponding quantities in the triangular field element.
    
    Parameters:
    
        ex = [x1 x2 x3]
        ey = [y1 y2 y3]         element coordinates
                                 
             D = [kxx kxy
                  kyx kyy]      constitutive matrix
    
             ed =[u1 u2 u3]     u1,u2,u3: nodal values
                  .. .. ..;
                  
    Returns:
    
        es=[ qx qy ] 
             ... ..]                element flows
    
        et=[ gx gy ]
             ... ..]                element gradients
    
    """

    if len(ex.shape) > 1:
        qs = np.zeros([ex.shape[0], 2])
        qt = np.zeros([ex.shape[0], 2])
        row = 0
        for exr, eyr, edr in zip(ex, ey, ed):
            exm = np.asmatrix(exr)
            eym = np.asmatrix(eyr)
            edm = np.asmatrix(edr)
            C = np.asmatrix(np.hstack([np.ones((3, 1)), exm.T, eym.T]))
            B = np.matrix([
                [0., 1., 0.],
                [0., 0., 1.]
            ])*C.I

            qs[row, :] = (-D*B*edm.T).T
            qt[row, :] = (B*edm.T).T
            row += 1

        return qs, qt
    else:
        exm = np.asmatrix(ex)
        eym = np.asmatrix(ey)
        edm = np.asmatrix(ed)
        C = np.asmatrix(np.hstack([np.ones((3, 1)), exm.T, eym.T]))
        B = np.matrix([
            [0., 1., 0.],
            [0., 0., 1.]
        ])*C.I

        qs = -D*B*edm.T
        qt = B*edm.T

        return qs.T, qt.T


def flw2qe(ex, ey, ep, D, eq=None):
    """
    Compute element stiffness (conductivity) matrix for a triangular field element.
    
    Parameters:
    
        ex = [x1, x2, x3, x4]
        ey = [y1, y2, y3, y4]   element coordinates
    
        ep = [t]                element thickness    

        D = [[kxx, kxy],
             [kyx, kyy]]        constitutive matrix
    
        eq                      heat supply per unit volume
             
    Returns:
    
        Ke                      element 'stiffness' matrix (4 x 4)

        fe                      element load vector (4 x 1)
    
    """
    xc = sum(ex)/4.
    yc = sum(ey)/4.

    K = np.zeros((5, 5))
    f = np.zeros((5, 1))

    if eq is None:
        k1 = flw2te([ex[0], ex[1], xc], [ey[0], ey[1], yc], ep, D)
        K = assem(np.array([1, 2, 5]), K, k1)
        k1 = flw2te([ex[1], ex[2], xc], [ey[1], ey[2], yc], ep, D)
        K = assem(np.array([2, 3, 5]), K, k1)
        k1 = flw2te([ex[2], ex[3], xc], [ey[2], ey[3], yc], ep, D)
        K = assem(np.array([3, 4, 5]), K, k1)
        k1 = flw2te([ex[3], ex[0], xc], [ey[3], ey[0], yc], ep, D)
        K = assem(np.array([4, 1, 5]), K, k1)
    else:
        k1, f1 = flw2te([ex[0], ex[1], xc], [ey[0], ey[1], yc], ep, D, eq)
        K, f = assem(np.array([1, 2, 5]), K, k1, f, f1)
        k1, f1 = flw2te([ex[1], ex[2], xc], [ey[1], ey[2], yc], ep, D, eq)
        K, f = assem(np.array([2, 3, 5]), K, k1, f, f1)
        k1, f1 = flw2te([ex[2], ex[3], xc], [ey[2], ey[3], yc], ep, D, eq)
        K, f = assem(np.array([3, 4, 5]), K, k1, f, f1)
        k1, f1 = flw2te([ex[3], ex[0], xc], [ey[3], ey[0], yc], ep, D, eq)
        K, f = assem(np.array([4, 1, 5]), K, k1, f, f1)
    Ke1, fe1 = statcon(K, f, np.array([5]))

    Ke = Ke1
    fe = fe1

    if eq is None:
        return Ke
    else:
        return Ke, fe


def flw2qs(ex, ey, ep, D, ed, eq=None):
    """
    Compute flows or corresponding quantities in the
    quadrilateral field element.
    
    Parameters:
    
        ex = [x1, x2, x3, x4]
        ey = [y1, y2, y3, y4]      element coordinates
    
        ep = [t]                   element thickness    

        D = [[kxx, kxy],
             [kyx, kyy]]           constitutive matrix

        ed = [[u1, u2, u3, u4],
              [.., .., .., ..]]    u1,u2,u3,u4: nodal values
    
        eq                         heat supply per unit volume
             
    Returns:
    
        es = [[qx, qy],
              [.., ..]]            element flows

        et = [[gx, gy],
              [.., ..]]            element gradients
    
    """
    K = np.zeros((5, 5))
    f = np.zeros((5, 1))

    xm = sum(ex)/4
    ym = sum(ey)/4

    if eq is None:
        q = 0
    else:
        q = eq

    En = np.array([
        [1, 2, 5],
        [2, 3, 5],
        [3, 4, 5],
        [4, 1, 5]
    ])
    ex1 = np.array([ex[0], ex[1], xm])
    ey1 = np.array([ey[0], ey[1], ym])
    ex2 = np.array([ex[1], ex[2], xm])
    ey2 = np.array([ey[1], ey[2], ym])
    ex3 = np.array([ex[2], ex[3], xm])
    ey3 = np.array([ey[2], ey[3], ym])
    ex4 = np.array([ex[3], ex[0], xm])
    ey4 = np.array([ey[3], ey[0], ym])

    if eq is None:
        k1 = flw2te(ex1, ey1, ep, D)
        K = assem(En[0], K, k1)
        k1 = flw2te(ex2, ey2, ep, D)
        K = assem(En[1], K, k1)
        k1 = flw2te(ex3, ey3, ep, D)
        K = assem(En[2], K, k1)
        k1 = flw2te(ex4, ey4, ep, D)
        K = assem(En[3], K, k1)
    else:
        k1, f1 = flw2te(ex1, ey1, ep, D, q)
        K, f = assem(En[0], K, k1, f, f1)
        k1, f1 = flw2te(ex2, ey2, ep, D, q)
        K, f = assem(En[1], K, k1, f, f1)
        k1, f1 = flw2te(ex3, ey3, ep, D, q)
        K, f = assem(En[2], K, k1, f, f1)
        k1, f1 = flw2te(ex4, ey4, ep, D, q)
        K, f = assem(En[3], K, k1, f, f1)

    if ed.ndim == 1:
        ed = np.array([ed])

    ni, nj = np.shape(ed)

    a = np.zeros((5, ni))
    for i in range(ni):
        a[np.ix_(range(5), [i])], r = np.asarray(
            solveq(K, f, np.arange(1, 5), ed[i]))

    s1, t1 = flw2ts(ex1, ey1, D, a[np.ix_(En[0, :]-1, np.arange(ni))].T)
    s2, t2 = flw2ts(ex2, ey2, D, a[np.ix_(En[1, :]-1, np.arange(ni))].T)
    s3, t3 = flw2ts(ex3, ey3, D, a[np.ix_(En[2, :]-1, np.arange(ni))].T)
    s4, t4 = flw2ts(ex4, ey4, D, a[np.ix_(En[3, :]-1, np.arange(ni))].T)

    es = (s1+s2+s3+s4)/4.
    et = (t1+t2+t3+t4)/4.

    return es, et


def flw2i4e(ex, ey, ep, D, eq=None):
    """
    Compute element stiffness (conductivity)
    matrix for 4 node isoparametric field element

    Parameters:
        
        ex = [x1 x2 x3 x4]  element coordinates
        ey = [y1 y2 y3 y4]

        ep = [t ir]         thickness and integration rule

        D  = [[kxx kxy],
              [kyx kyy]]    constitutive matrix

        eq                  heat supply per unit volume

    Returns:
        Ke                  element 'stiffness' matrix (4 x 4)
        fe                  element load vector (4 x 1)

    """
    t = ep[0]
    ir = ep[1]
    ngp = ir*ir

    if eq is None:
        q = 0
    else:
        q = eq

    if ir == 1:
        g1 = 0.0
        w1 = 2.0
        gp = np.mat([g1, g1])
        w = np.mat([w1, w1])
    elif ir == 2:
        g1 = 0.577350269189626
        w1 = 1
        gp = np.mat([
            [-g1, -g1],
            [g1, -g1],
            [-g1, g1],
            [g1, g1]
        ])
        w = np.mat([
            [w1, w1],
            [w1, w1],
            [w1, w1],
            [w1, w1]
        ])
    elif ir == 3:
        g1 = 0.774596669241483
        g2 = 0.
        w1 = 0.555555555555555
        w2 = 0.888888888888888
        gp = np.mat([
            [-g1, -g1],
            [-g2, -g1],
            [g1, -g1],
            [-g1, g2],
            [g2, g2],
            [g1, g2],
            [-g1, g1],
            [g2, g1],
            [g1, g1]
        ])
        w = np.mat([
            [w1, w1],
            [w2, w1],
            [w1, w1],
            [w1, w2],
            [w2, w2],
            [w1, w2],
            [w1, w1],
            [w2, w1],
            [w1, w1]
        ])
    else:
        info("Used number of integration points not implemented")
    wp = np.multiply(w[:, 0], w[:, 1])

    xsi = gp[:, 0]
    eta = gp[:, 1]
    r2 = ngp*2

    N = np.multiply((1-xsi), (1-eta))/4.
    N = np.append(N, np.multiply((1+xsi), (1-eta))/4., axis=1)
    N = np.append(N, np.multiply((1+xsi), (1+eta))/4., axis=1)
    N = np.append(N, np.multiply((1-xsi), (1+eta))/4., axis=1)

    dNr = np.mat(np.zeros((r2, 4)))
    dNr[0:r2:2, 0] = -(1-eta)/4.
    dNr[0:r2:2, 1] = (1-eta)/4.
    dNr[0:r2:2, 2] = (1+eta)/4.
    dNr[0:r2:2, 3] = -(1+eta)/4.
    dNr[1:r2+1:2, 0] = -(1-xsi)/4.
    dNr[1:r2+1:2, 1] = -(1+xsi)/4.
    dNr[1:r2+1:2, 2] = (1+xsi)/4.
    dNr[1:r2+1:2, 3] = (1-xsi)/4.

    Ke1 = np.mat(np.zeros((4, 4)))
    fe1 = np.mat(np.zeros((4, 1)))
    JT = dNr*np.mat([ex, ey]).T

    for i in range(ngp):
        indx = np.array([2*(i+1)-1, 2*(i+1)])
        detJ = np.linalg.det(JT[indx-1, :])
        if detJ < 10*np.finfo(float).eps:
            info("Jacobi determinant == 0")
        JTinv = np.linalg.inv(JT[indx-1, :])
        B = JTinv*dNr[indx-1, :]
        Ke1 = Ke1+B.T*D*B*detJ*wp[i].item()
        fe1 = fe1+N[i, :].T*detJ*wp[i]

    if eq is None:
        return Ke1*t
    else:
        return Ke1*t, fe1*t*eq


def flw2i4s(ex, ey, ep, D, ed):
    """
    Compute flows or corresponding quantities in the
    4 node isoparametric element.
    
    Parameters:
        
        ex = [x1 x2 x3 x4]         element coordinates
        ey = [y1 y2 y3 y4]

        ep = [t ir]                thickness and integration rule

        D  = [[kxx kxy],
              [kyx kyy]]           constitutive matrix

        ed = [u1, u2, u3, u4]      u1,u2,u3,u4: nodal values

    Returns:
        es = [[qx, qy],
              [.., ..]]             element flows

        et = [[qx, qy],
              [... ..]]             element gradients

        eci=[[ix1, iy1],            Gauss point location vector
             [...  ...],            nint: number of integration points
             [ix(nint), iy(nint)]

    """
    t = ep[0]
    ir = ep[1]
    ngp = ir*ir

    if ir == 1:
        g1 = 0.0
        w1 = 2.0
        gp = np.mat([g1, g1])
        w = np.mat([w1, w1])
    elif ir == 2:
        g1 = 0.577350269189626
        w1 = 1
        gp = np.mat([
            [-g1, -g1],
            [g1, -g1],
            [-g1, g1],
            [g1, g1]
        ])
        w = np.mat([
            [w1, w1],
            [w1, w1],
            [w1, w1],
            [w1, w1]
        ])
    elif ir == 3:
        g1 = 0.774596669241483
        g2 = 0.
        w1 = 0.555555555555555
        w2 = 0.888888888888888
        gp = np.mat([
            [-g1, -g1],
            [-g2, -g1],
            [g1, -g1],
            [-g1, g2],
            [g2, g2],
            [g1, g2],
            [-g1, g1],
            [g2, g1],
            [g1, g1]
        ])
        w = np.mat([
            [w1, w1],
            [w2, w1],
            [w1, w1],
            [w1, w2],
            [w2, w2],
            [w1, w2],
            [w1, w1],
            [w2, w1],
            [w1, w1]
        ])
    else:
        info("Used number of integration points not implemented")
    wp = np.multiply(w[:, 0], w[:, 1])

    xsi = gp[:, 0]
    eta = gp[:, 1]
    r2 = ngp*2

    N = np.multiply((1-xsi), (1-eta))/4.
    N = np.append(N, np.multiply((1+xsi), (1-eta))/4., axis=1)
    N = np.append(N, np.multiply((1+xsi), (1+eta))/4., axis=1)
    N = np.append(N, np.multiply((1-xsi), (1+eta))/4., axis=1)

    dNr = np.mat(np.zeros((r2, 4)))
    dNr[0:r2:2, 0] = -(1-eta)/4.
    dNr[0:r2:2, 1] = (1-eta)/4.
    dNr[0:r2:2, 2] = (1+eta)/4.
    dNr[0:r2:2, 3] = -(1+eta)/4.
    dNr[1:r2+1:2, 0] = -(1-xsi)/4.
    dNr[1:r2+1:2, 1] = -(1+xsi)/4.
    dNr[1:r2+1:2, 2] = (1+xsi)/4.
    dNr[1:r2+1:2, 3] = (1-xsi)/4.

    eci = N*np.mat([ex, ey]).T
    if ed.ndim == 1:
        ed = np.array([ed])

    red, ced = np.shape(ed)
    JT = dNr*np.mat([ex, ey]).T

    es = np.mat(np.zeros((ngp*red, 2)))
    et = np.mat(np.zeros((ngp*red, 2)))
    for i in range(ngp):
        indx = np.array([2*(i+1)-1, 2*(i+1)])
        detJ = np.linalg.det(JT[indx-1, :])
        if detJ < 10*np.finfo(float).eps:
            info("Jacobi determinatn == 0")
        JTinv = np.linalg.inv(JT[indx-1, :])
        B = JTinv*dNr[indx-1, :]
        p1 = -D*B*ed.T
        p2 = B*ed.T
        es[i:ngp*red:ngp, :] = p1.T
        et[i:ngp*red:ngp, :] = p2.T

    return es, et, eci


def flw2i8e(ex, ey, ep, D, eq=None):
    """
    Compute element stiffness (conductivity)
    matrix for 8 node isoparametric field element.
    
    Parameters:
    
        ex = [x1, ..., x8]      element coordinates
        ey = [y1, ..., y8]
        
        ep = [t, ir]            thickness and integration rule

        D = [[kxx, kxy],
             [kyx, kyy]]        constitutive matrix

        eq                      heat supply per unit volume

    Returns:
    
        Ke                      element 'stiffness' matrix (8 x 8)
        fe                      element load vector (8 x 1)

    """
    t = ep[0]
    ir = ep[1]
    ngp = ir*ir

    if eq is None:
        q = 0
    else:
        q = eq

    if ir == 1:
        g1 = 0.0
        w1 = 2.0
        gp = np.mat([g1, g1])
        w = np.mat([w1, w1])
    elif ir == 2:
        g1 = 0.577350269189626
        w1 = 1
        gp = np.mat([
            [-g1, -g1],
            [g1, -g1],
            [-g1, g1],
            [g1, g1]
        ])
        w = np.mat([
            [w1, w1],
            [w1, w1],
            [w1, w1],
            [w1, w1]
        ])
    elif ir == 3:
        g1 = 0.774596669241483
        g2 = 0.
        w1 = 0.555555555555555
        w2 = 0.888888888888888
        gp = np.mat([
            [-g1, -g1],
            [-g2, -g1],
            [g1, -g1],
            [-g1, g2],
            [g2, g2],
            [g1, g2],
            [-g1, g1],
            [g2, g1],
            [g1, g1]
        ])
        w = np.mat([
            [w1, w1],
            [w2, w1],
            [w1, w1],
            [w1, w2],
            [w2, w2],
            [w1, w2],
            [w1, w1],
            [w2, w1],
            [w1, w1]
        ])
    else:
        info("Used number of integration points not implemented")
    wp = np.multiply(w[:, 0], w[:, 1])

    xsi = gp[:, 0]
    eta = gp[:, 1]
    r2 = ngp*2

    N = np.multiply(np.multiply(-(1-xsi), (1-eta)), (1+xsi+eta))/4.
    N = np.append(N, np.multiply(
        np.multiply(-(1+xsi), (1-eta)), (1-xsi+eta))/4., axis=1)
    N = np.append(N, np.multiply(
        np.multiply(-(1+xsi), (1+eta)), (1-xsi-eta))/4., axis=1)
    N = np.append(N, np.multiply(
        np.multiply(-(1-xsi), (1+eta)), (1+xsi-eta))/4., axis=1)
    N = np.append(N, np.multiply(
        (1-np.multiply(xsi, xsi)), (1-eta))/2., axis=1)
    N = np.append(N, np.multiply(
        (1+xsi), (1-np.multiply(eta, eta)))/2., axis=1)
    N = np.append(N, np.multiply(
        (1-np.multiply(xsi, xsi)), (1+eta))/2., axis=1)
    N = np.append(N, np.multiply(
        (1-xsi), (1-np.multiply(eta, eta)))/2., axis=1)

    dNr = np.mat(np.zeros((r2, 8)))
    dNr[0:r2:2, 0] = -(-np.multiply((1-eta), (1+xsi+eta)) +
                       np.multiply((1-xsi), (1-eta)))/4.
    dNr[0:r2:2, 1] = -(np.multiply((1-eta), (1-xsi+eta)) -
                       np.multiply((1+xsi), (1-eta)))/4.
    dNr[0:r2:2, 2] = -(np.multiply((1+eta), (1-xsi-eta)) -
                       np.multiply((1+xsi), (1+eta)))/4.
    dNr[0:r2:2, 3] = -(-np.multiply((1+eta), (1+xsi-eta)) +
                       np.multiply((1-xsi), (1+eta)))/4.
    dNr[0:r2:2, 4] = -np.multiply(xsi, (1-eta))
    dNr[0:r2:2, 5] = (1-np.multiply(eta, eta))/2.
    dNr[0:r2:2, 6] = -np.multiply(xsi, (1+eta))
    dNr[0:r2:2, 7] = -(1-np.multiply(eta, eta))/2.
    dNr[1:r2+1:2, 0] = -(-np.multiply((1-xsi), (1+xsi+eta)) +
                         np.multiply((1-xsi), (1-eta)))/4.
    dNr[1:r2+1:2, 1] = -(-np.multiply((1+xsi), (1-xsi+eta)) +
                         np.multiply((1+xsi), (1-eta)))/4.
    dNr[1:r2+1:2, 2] = -(np.multiply((1+xsi), (1-xsi-eta)) -
                         np.multiply((1+xsi), (1+eta)))/4.
    dNr[1:r2+1:2, 3] = -(np.multiply((1-xsi), (1+xsi-eta)) -
                         np.multiply((1-xsi), (1+eta)))/4.
    dNr[1:r2+1:2, 4] = -(1-np.multiply(xsi, xsi))/2.
    dNr[1:r2+1:2, 5] = -np.multiply(eta, (1+xsi))
    dNr[1:r2+1:2, 6] = (1-np.multiply(xsi, xsi))/2.
    dNr[1:r2+1:2, 7] = -np.multiply(eta, (1-xsi))

    Ke1 = np.mat(np.zeros((8, 8)))
    fe1 = np.mat(np.zeros((8, 1)))
    JT = dNr*np.mat([ex, ey]).T

    for i in range(ngp):
        indx = np.array([2*(i+1)-1, 2*(i+1)])
        detJ = np.linalg.det(JT[indx-1, :])
        if detJ < 10*np.finfo(float).eps:
            info("Jacobideterminanten lika med noll!")
        JTinv = np.linalg.inv(JT[indx-1, :])
        B = JTinv*dNr[indx-1, :]
        Ke1 = Ke1+B.T*D*B*detJ*wp[i].item()
        fe1 = fe1+N[i, :].T*detJ*wp[i]

    if eq != None:
        return Ke1*t, fe1*t*q
    else:
        return Ke1*t


def flw2i8s(ex, ey, ep, D, ed):
    """
    Compute flows or corresponding quantities in the
    8 node isoparametric element.
    
    Parameters:
        
        ex = [x1,x2,x3....,x8]     element coordinates
        ey = [y1,y2,y3....,y8]

        ep = [t,ir]                thickness and integration rule

        D  = [[kxx,kxy],
              [kyx,kyy]]           constitutive matrix

        ed = [u1,....,u8]          u1,....,u8: nodal values

    Returns:
        es = [[qx,qy],
              [..,..]]             element flows

        et = [[qx,qy],
              [..,..]]             element gradients

        eci=[[ix1,iy1],            Gauss point location vector
             [...,...],            nint: number of integration points
             [ix(nint),iy(nint)]]

    """
    t = ep[0]
    ir = ep[1]
    ngp = ir*ir

    if ir == 1:
        g1 = 0.0
        w1 = 2.0
        gp = np.mat([g1, g1])
        w = np.mat([w1, w1])
    elif ir == 2:
        g1 = 0.577350269189626
        w1 = 1
        gp = np.mat([
            [-g1, -g1],
            [g1, -g1],
            [-g1, g1],
            [g1, g1]
        ])
        w = np.mat([
            [w1, w1],
            [w1, w1],
            [w1, w1],
            [w1, w1]
        ])
    elif ir == 3:
        g1 = 0.774596669241483
        g2 = 0.
        w1 = 0.555555555555555
        w2 = 0.888888888888888
        gp = np.mat([
            [-g1, -g1],
            [-g2, -g1],
            [g1, -g1],
            [-g1, g2],
            [g2, g2],
            [g1, g2],
            [-g1, g1],
            [g2, g1],
            [g1, g1]
        ])
        w = np.mat([
            [w1, w1],
            [w2, w1],
            [w1, w1],
            [w1, w2],
            [w2, w2],
            [w1, w2],
            [w1, w1],
            [w2, w1],
            [w1, w1]
        ])
    else:
        info("Used number of integration points not implemented")
    wp = np.multiply(w[:, 0], w[:, 1])

    xsi = gp[:, 0]
    eta = gp[:, 1]
    r2 = ngp*2

    N = np.multiply(np.multiply(-(1-xsi), (1-eta)), (1+xsi+eta))/4.
    N = np.append(N, np.multiply(
        np.multiply(-(1+xsi), (1-eta)), (1-xsi+eta))/4., axis=1)
    N = np.append(N, np.multiply(
        np.multiply(-(1+xsi), (1+eta)), (1-xsi-eta))/4., axis=1)
    N = np.append(N, np.multiply(
        np.multiply(-(1-xsi), (1+eta)), (1+xsi-eta))/4., axis=1)
    N = np.append(N, np.multiply(
        (1-np.multiply(xsi, xsi)), (1-eta))/2., axis=1)
    N = np.append(N, np.multiply(
        (1+xsi), (1-np.multiply(eta, eta)))/2., axis=1)
    N = np.append(N, np.multiply(
        (1-np.multiply(xsi, xsi)), (1+eta))/2., axis=1)
    N = np.append(N, np.multiply(
        (1-xsi), (1-np.multiply(eta, eta)))/2., axis=1)

    dNr = np.mat(np.zeros((r2, 8)))
    dNr[0:r2:2, 0] = -(-np.multiply((1-eta), (1+xsi+eta)) +
                       np.multiply((1-xsi), (1-eta)))/4.
    dNr[0:r2:2, 1] = -(np.multiply((1-eta), (1-xsi+eta)) -
                       np.multiply((1+xsi), (1-eta)))/4.
    dNr[0:r2:2, 2] = -(np.multiply((1+eta), (1-xsi-eta)) -
                       np.multiply((1+xsi), (1+eta)))/4.
    dNr[0:r2:2, 3] = -(-np.multiply((1+eta), (1+xsi-eta)) +
                       np.multiply((1-xsi), (1+eta)))/4.
    dNr[0:r2:2, 4] = -np.multiply(xsi, (1-eta))
    dNr[0:r2:2, 5] = (1-np.multiply(eta, eta))/2.
    dNr[0:r2:2, 6] = -np.multiply(xsi, (1+eta))
    dNr[0:r2:2, 7] = -(1-np.multiply(eta, eta))/2.
    dNr[1:r2+1:2, 0] = -(-np.multiply((1-xsi), (1+xsi+eta)) +
                         np.multiply((1-xsi), (1-eta)))/4.
    dNr[1:r2+1:2, 1] = -(-np.multiply((1+xsi), (1-xsi+eta)) +
                         np.multiply((1+xsi), (1-eta)))/4.
    dNr[1:r2+1:2, 2] = -(np.multiply((1+xsi), (1-xsi-eta)) -
                         np.multiply((1+xsi), (1+eta)))/4.
    dNr[1:r2+1:2, 3] = -(np.multiply((1-xsi), (1+xsi-eta)) -
                         np.multiply((1-xsi), (1+eta)))/4.
    dNr[1:r2+1:2, 4] = -(1-np.multiply(xsi, xsi))/2.
    dNr[1:r2+1:2, 5] = -np.multiply(eta, (1+xsi))
    dNr[1:r2+1:2, 6] = (1-np.multiply(xsi, xsi))/2.
    dNr[1:r2+1:2, 7] = -np.multiply(eta, (1-xsi))

    eci = N*np.mat([ex, ey]).T
    if ed.ndim == 1:
        ed = np.array([ed])
    red, ced = np.shape(ed)
    JT = dNr*np.mat([ex, ey]).T

    es = np.mat(np.zeros((ngp*red, 2)))
    et = np.mat(np.zeros((ngp*red, 2)))

    for i in range(ngp):
        indx = np.array([2*(i+1)-1, 2*(i+1)])
        detJ = np.linalg.det(JT[indx-1, :])
        if detJ < 10*np.finfo(float).eps:
            info("Jacobi determinant == 0")
        JTinv = np.linalg.inv(JT[indx-1, :])
        B = JTinv*dNr[indx-1, :]
        p1 = -D*B*ed.T
        p2 = B*ed.T
        es[i:ngp*red:ngp, :] = p1.T
        et[i:ngp*red:ngp, :] = p2.T

    return es, et, eci


def flw3i8e(ex, ey, ez, ep, D, eq=None):
    """
    Compute element stiffness (conductivity)
    matrix for 8 node isoparametric field element.
    
    Parameters:
    
        ex = [x1,x2,x3,...,x8]
        ey = [y1,y2,y3,...,y8]      element coordinates
        ez = [z1,z2,z3,...,z8]

        ep = [ir]                   Ir: Integration rule

        D = [[kxx,kxy,kxz],
             [kyx,kyy,kyz],
             [kzx,kzy,kzz]]         constitutive matrix

        eq                          heat supply per unit volume

    Output:

        Ke                          element 'stiffness' matrix (8 x 8)
        fe                          element load vector (8 x 1)

    """
    ir = ep[0]
    ngp = ir*ir*ir

    if eq is None:
        q = 0
    else:
        q = eq

    if ir == 2:
        g1 = 0.577350269189626
        w1 = 1
        gp = np.mat([
            [-1, -1, -1],
            [1, -1, -1],
            [1, 1, -1],
            [-1, 1, -1],
            [-1, -1, 1],
            [1, -1, 1],
            [1, 1, 1],
            [-1, 1, 1]
        ])*g1
        w = np.mat(np.ones((8, 3)))*w1
    elif ir == 3:
        g1 = 0.774596669241483
        g2 = 0.
        w1 = 0.555555555555555
        w2 = 0.888888888888888
        gp = np.mat(np.zeros((27, 3)))
        w = np.mat(np.zeros((27, 3)))
        I1 = np.array([-1, 0, 1, -1, 0, 1, -1, 0, 1])
        I2 = np.array([0, -1, 0, 0, 1, 0, 0, 1, 0])
        gp[:, 0] = np.mat([I1, I1, I1]).reshape(27, 1)*g1
        gp[:, 0] = np.mat([I2, I2, I2]).reshape(27, 1)*g2+gp[:, 0]
        I1 = abs(I1)
        I2 = abs(I2)
        w[:, 0] = np.mat([I1, I1, I1]).reshape(27, 1)*w1
        w[:, 0] = np.mat([I2, I2, I2]).reshape(27, 1)*w2+w[:, 0]
        I1 = np.array([-1, -1, -1, 0, 0, 0, 1, 1, 1])
        I2 = np.array([0, 0, 0, 1, 1, 1, 0, 0, 0])
        gp[:, 1] = np.mat([I1, I1, I1]).reshape(27, 1)*g1
        gp[:, 1] = np.mat([I2, I2, I2]).reshape(27, 1)*g2+gp[:, 1]
        I1 = abs(I1)
        I2 = abs(I2)
        w[:, 1] = np.mat([I1, I1, I1]).reshape(27, 1)*w1
        w[:, 1] = np.mat([I2, I2, I2]).reshape(27, 1)*w2+w[:, 1]
        I1 = np.array([-1, -1, -1, -1, -1, -1, -1, -1, -1])
        I2 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])
        I3 = abs(I1)
        gp[:, 2] = np.mat([I1, I2, I3]).reshape(27, 1)*g1
        gp[:, 2] = np.mat([I2, I3, I2]).reshape(27, 1)*g2+gp[:, 2]
        w[:, 2] = np.mat([I3, I2, I3]).reshape(27, 1)*w1
        w[:, 2] = np.mat([I2, I3, I2]).reshape(27, 1)*w2+w[:, 2]
    else:
        info("Used number of integration points not implemented")
        return

    wp = np.multiply(np.multiply(w[:, 0], w[:, 1]), w[:, 2])

    xsi = gp[:, 0]
    eta = gp[:, 1]
    zet = gp[:, 2]
    r2 = ngp*3

    N = np.multiply(np.multiply((1-xsi), (1-eta)), (1-zet))/8.
    N = np.append(N, np.multiply(np.multiply(
        (1+xsi), (1-eta)), (1-zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1+xsi), (1+eta)), (1-zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1-xsi), (1+eta)), (1-zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1-xsi), (1-eta)), (1+zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1+xsi), (1-eta)), (1+zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1+xsi), (1+eta)), (1+zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1-xsi), (1+eta)), (1+zet))/8., axis=1)

    dNr = np.mat(np.zeros((r2, 8)))
    dNr[0:r2:3, 0] = np.multiply(-(1-eta), (1-zet))
    dNr[0:r2:3, 1] = np.multiply((1-eta), (1-zet))
    dNr[0:r2:3, 2] = np.multiply((1+eta), (1-zet))
    dNr[0:r2:3, 3] = np.multiply(-(1+eta), (1-zet))
    dNr[0:r2:3, 4] = np.multiply(-(1-eta), (1+zet))
    dNr[0:r2:3, 5] = np.multiply((1-eta), (1+zet))
    dNr[0:r2:3, 6] = np.multiply((1+eta), (1+zet))
    dNr[0:r2:3, 7] = np.multiply(-(1+eta), (1+zet))
    dNr[1:r2+1:3, 0] = np.multiply(-(1-xsi), (1-zet))
    dNr[1:r2+1:3, 1] = np.multiply(-(1+xsi), (1-zet))
    dNr[1:r2+1:3, 2] = np.multiply((1+xsi), (1-zet))
    dNr[1:r2+1:3, 3] = np.multiply((1-xsi), (1-zet))
    dNr[1:r2+1:3, 4] = np.multiply(-(1-xsi), (1+zet))
    dNr[1:r2+1:3, 5] = np.multiply(-(1+xsi), (1+zet))
    dNr[1:r2+1:3, 6] = np.multiply((1+xsi), (1+zet))
    dNr[1:r2+1:3, 7] = np.multiply((1-xsi), (1+zet))
    dNr[2:r2+2:3, 0] = np.multiply(-(1-xsi), (1-eta))
    dNr[2:r2+2:3, 1] = np.multiply(-(1+xsi), (1-eta))
    dNr[2:r2+2:3, 2] = np.multiply(-(1+xsi), (1+eta))
    dNr[2:r2+2:3, 3] = np.multiply(-(1-xsi), (1+eta))
    dNr[2:r2+2:3, 4] = np.multiply((1-xsi), (1-eta))
    dNr[2:r2+2:3, 5] = np.multiply((1+xsi), (1-eta))
    dNr[2:r2+2:3, 6] = np.multiply((1+xsi), (1+eta))
    dNr[2:r2+2:3, 7] = np.multiply((1-xsi), (1+eta))
    dNr = dNr/8.

    Ke1 = np.mat(np.zeros((8, 8)))
    fe1 = np.mat(np.zeros((8, 1)))
    JT = dNr*np.mat([ex, ey, ez]).T

    for i in range(ngp):
        indx = np.array([3*(i+1)-2, 3*(i+1)-1, 3*(i+1)])
        detJ = np.linalg.det(JT[indx-1, :])
        if detJ < 10*np.finfo(float).eps:
            info("Jacobi determinant == 0")
        JTinv = np.linalg.inv(JT[indx-1, :])
        B = JTinv*dNr[indx-1, :]
        Ke1 = Ke1+B.T*D*B*detJ*wp[i].item()
        fe1 = fe1+N[i, :].T*detJ*wp[i]

    if eq != None:
        return Ke1, fe1*q
    else:
        return Ke1


def flw3i8s(ex, ey, ez, ep, D, ed):
    """
    Compute flows or corresponding quantities in the
    8 node (3-dim) isoparametric field element.
    
    Parameters:
    
        ex = [x1,x2,x3,...,x8]
        ey = [y1,y2,y3,...,y8]              element coordinates
        ez = [z1,z2,z3,...,z8]

        ep = [ir]                           Ir: Integration rule

        D = [[kxx,kxy,kxz],
             [kyx,kyy,kyz],
             [kzx,kzy,kzz]]                 constitutive matrix

        ed = [[u1,....,u8],                 element nodal values
              [..,....,..]]

    Output:

        es = [[qx,qy,qz],
              [..,..,..]]                   element flows(s)

        et = [[qx,qy,qz],                   element gradients(s)
              [..,..,..]]

        eci = [[ix1,ix1,iz1],               location vector
               [...,...,...],               nint: number of integration points
               [ix(nint),iy(nint),iz(nint)]]

    """
    ir = ep[0]
    ngp = ir*ir*ir

    if ir == 2:
        g1 = 0.577350269189626
        w1 = 1
        gp = np.mat([
            [-1, -1, -1],
            [1, -1, -1],
            [1, 1, -1],
            [-1, 1, -1],
            [-1, -1, 1],
            [1, -1, 1],
            [1, 1, 1],
            [-1, 1, 1]
        ])*g1
        w = np.mat(np.ones((8, 3)))*w1
    elif ir == 3:
        g1 = 0.774596669241483
        g2 = 0.
        w1 = 0.555555555555555
        w2 = 0.888888888888888
        gp = np.mat(np.zeros((27, 3)))
        w = np.mat(np.zeros((27, 3)))
        I1 = np.array([-1, 0, 1, -1, 0, 1, -1, 0, 1])
        I2 = np.array([0, -1, 0, 0, 1, 0, 0, 1, 0])
        gp[:, 0] = np.mat([I1, I1, I1]).reshape(27, 1)*g1
        gp[:, 0] = np.mat([I2, I2, I2]).reshape(27, 1)*g2+gp[:, 0]
        I1 = abs(I1)
        I2 = abs(I2)
        w[:, 0] = np.mat([I1, I1, I1]).reshape(27, 1)*w1
        w[:, 0] = np.mat([I2, I2, I2]).reshape(27, 1)*w2+w[:, 0]
        I1 = np.array([-1, -1, -1, 0, 0, 0, 1, 1, 1])
        I2 = np.array([0, 0, 0, 1, 1, 1, 0, 0, 0])
        gp[:, 1] = np.mat([I1, I1, I1]).reshape(27, 1)*g1
        gp[:, 1] = np.mat([I2, I2, I2]).reshape(27, 1)*g2+gp[:, 1]
        I1 = abs(I1)
        I2 = abs(I2)
        w[:, 1] = np.mat([I1, I1, I1]).reshape(27, 1)*w1
        w[:, 1] = np.mat([I2, I2, I2]).reshape(27, 1)*w2+w[:, 1]
        I1 = np.array([-1, -1, -1, -1, -1, -1, -1, -1, -1])
        I2 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])
        I3 = abs(I1)
        gp[:, 2] = np.mat([I1, I2, I3]).reshape(27, 1)*g1
        gp[:, 2] = np.mat([I2, I3, I2]).reshape(27, 1)*g2+gp[:, 2]
        w[:, 2] = np.mat([I3, I2, I3]).reshape(27, 1)*w1
        w[:, 2] = np.mat([I2, I3, I2]).reshape(27, 1)*w2+w[:, 2]
    else:
        info("Used number of integration points not implemented")
        return

    wp = np.multiply(np.multiply(w[:, 0], w[:, 1]), w[:, 2])

    xsi = gp[:, 0]
    eta = gp[:, 1]
    zet = gp[:, 2]
    r2 = ngp*3

    N = np.multiply(np.multiply((1-xsi), (1-eta)), (1-zet))/8.
    N = np.append(N, np.multiply(np.multiply(
        (1+xsi), (1-eta)), (1-zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1+xsi), (1+eta)), (1-zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1-xsi), (1+eta)), (1-zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1-xsi), (1-eta)), (1+zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1+xsi), (1-eta)), (1+zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1+xsi), (1+eta)), (1+zet))/8., axis=1)
    N = np.append(N, np.multiply(np.multiply(
        (1-xsi), (1+eta)), (1+zet))/8., axis=1)

    dNr = np.mat(np.zeros((r2, 8)))
    dNr[0:r2:3, 0] = np.multiply(-(1-eta), (1-zet))
    dNr[0:r2:3, 1] = np.multiply((1-eta), (1-zet))
    dNr[0:r2:3, 2] = np.multiply((1+eta), (1-zet))
    dNr[0:r2:3, 3] = np.multiply(-(1+eta), (1-zet))
    dNr[0:r2:3, 4] = np.multiply(-(1-eta), (1+zet))
    dNr[0:r2:3, 5] = np.multiply((1-eta), (1+zet))
    dNr[0:r2:3, 6] = np.multiply((1+eta), (1+zet))
    dNr[0:r2:3, 7] = np.multiply(-(1+eta), (1+zet))
    dNr[1:r2+1:3, 0] = np.multiply(-(1-xsi), (1-zet))
    dNr[1:r2+1:3, 1] = np.multiply(-(1+xsi), (1-zet))
    dNr[1:r2+1:3, 2] = np.multiply((1+xsi), (1-zet))
    dNr[1:r2+1:3, 3] = np.multiply((1-xsi), (1-zet))
    dNr[1:r2+1:3, 4] = np.multiply(-(1-xsi), (1+zet))
    dNr[1:r2+1:3, 5] = np.multiply(-(1+xsi), (1+zet))
    dNr[1:r2+1:3, 6] = np.multiply((1+xsi), (1+zet))
    dNr[1:r2+1:3, 7] = np.multiply((1-xsi), (1+zet))
    dNr[2:r2+2:3, 0] = np.multiply(-(1-xsi), (1-eta))
    dNr[2:r2+2:3, 1] = np.multiply(-(1+xsi), (1-eta))
    dNr[2:r2+2:3, 2] = np.multiply(-(1+xsi), (1+eta))
    dNr[2:r2+2:3, 3] = np.multiply(-(1-xsi), (1+eta))
    dNr[2:r2+2:3, 4] = np.multiply((1-xsi), (1-eta))
    dNr[2:r2+2:3, 5] = np.multiply((1+xsi), (1-eta))
    dNr[2:r2+2:3, 6] = np.multiply((1+xsi), (1+eta))
    dNr[2:r2+2:3, 7] = np.multiply((1-xsi), (1+eta))
    dNr = dNr/8.

    eci = N*np.mat([ex, ey, ez]).T
    if ed.ndim == 1:
        ed = np.array([ed])
        red, ced = np.shape(ed)
    JT = dNr*np.mat([ex, ey, ez]).T

    es = np.mat(np.zeros((ngp*red, 3)))
    et = np.mat(np.zeros((ngp*red, 3)))
    for i in range(ngp):
        indx = np.array([3*(i+1)-2, 3*(i+1)-1, 3*(i+1)])
        detJ = np.linalg.det(JT[indx-1, :])
        if detJ < 10*np.finfo(float).eps:
            info("Jacobideterminanten lika med noll!")
        JTinv = np.linalg.inv(JT[indx-1, :])
        B = JTinv*dNr[indx-1, :]
        p1 = -D*B*ed.T
        p2 = B*ed.T
        es[i:ngp*red:ngp, :] = p1.T
        et[i:ngp*red:ngp, :] = p2.T

    return es, et, eci


def plante(ex, ey, ep, D, eq=None):
    """
    Calculate the stiffness matrix for a triangular plane stress or plane strain element.
    
    Parameters:
    
        ex = [x1,x2,x3]         element coordinates
        ey = [y1,y2,y3]
     
        ep = [ptype,t]          ptype: analysis type
                                t: thickness
     
        D                       constitutive matrix
    
        eq = [[bx],               bx: body force x-dir
              [by]]               by: body force y-dir
              
    Returns:
    
        Ke                      element stiffness matrix (6 x 6)
        fe                      equivalent nodal forces (6 x 1) (if eq is given)

    """

    ptype, t = ep

    bx = 0.0
    by = 0.0

    if not eq is None:
        bx = eq[0]
        by = eq[1]

    C = np.mat([
        [1, ex[0], ey[0], 0,     0,     0],
        [0,     0,     0, 1, ex[0], ey[0]],
        [1, ex[1], ey[1], 0,     0,     0],
        [0,     0,     0, 1, ex[1], ey[1]],
        [1, ex[2], ey[2], 0,     0,     0],
        [0,     0,     0, 1, ex[2], ey[2]]
    ])

    A = 0.5*np.linalg.det(np.mat([
        [1, ex[0], ey[0]],
        [1, ex[1], ey[1]],
        [1, ex[2], ey[2]]
    ]))

    # --------- plane stress --------------------------------------

    if ptype == 1:
        B = np.mat([
            [0, 1, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 1],
            [0, 0, 1, 0, 1, 0]
        ])*np.linalg.inv(C)

        colD = D.shape[1]

        if colD > 3:
            Cm = np.linalg.inv(D)
            Dm = np.linalg.inv(Cm[np.ix_((0, 1, 3), (0, 1, 3))])
        else:
            Dm = D

        Ke = B.T*Dm*B*A*t
        fe = A/3*np.mat([bx, by, bx, by, bx, by]).T*t

        if eq is None:
            return Ke
        else:
            return Ke, fe.T

    #--------- plane strain --------------------------------------

    elif ptype == 2:
        B = np.mat([
            [0, 1, 0, 0, 0, 0, ],
            [0, 0, 0, 0, 0, 1, ],
            [0, 0, 1, 0, 1, 0, ]
        ])*np.linalg.inv(C)

        colD = D.shape[1]

        if colD > 3:
            Dm = D[np.ix_((0, 1, 3), (0, 1, 3))]
        else:
            Dm = D

        Ke = B.T*Dm*B*A*t
        fe = A/3*np.mat([bx, by, bx, by, bx, by]).T*t

        if eq is None:
            return Ke
        else:
            return Ke, fe.T

    else:
        info("Error ! Check first argument, ptype=1 or 2 allowed")
        if eq is None:
            return None
        else:
            return None, None


def plants(ex, ey, ep, D, ed):
    """
    Calculate element normal and shear stress for a
    triangular plane stress or plane strain element.
    
    INPUT:  ex = [x1 x2 x3]         element coordinates
           ey = [y1 y2 y3]
    
           ep = [ptype t ]         ptype: analysis type
                                   t: thickness
    
           D                       constitutive matrix
    
           ed =[u1 u2 ...u6        element displacement vector
                ......     ]       one row for each element
    
    OUTPUT: es = [ sigx sigy [sigz] tauxy   element stress matrix
                 ......                 ]  one row for each element
    
           et = [ epsx epsy [epsz] gamxy   element strain matrix
                 ......                 ]  one row for each element
    """

    ptype = ep[0]

    if np.ndim(ex) == 1:
        ex = np.array([ex])
    if np.ndim(ey) == 1:
        ey = np.array([ey])
    if np.ndim(ed) == 1:
        ed = np.array([ed])

    rowed = ed.shape[0]
    rowex = ex.shape[0]

    # --------- plane stress --------------------------------------

    if ptype == 1:

        colD = D.shape[1]

        if colD > 3:
            Cm = np.linalg.inv(D)
            Dm = np.linalg.inv(Cm[np.ix_((0, 1, 3), (0, 1, 3))])
        else:
            Dm = D

        incie = 0

        if rowex == 1:
            incie = 0
        else:
            incie = 1

        et = np.zeros([rowed, colD])
        es = np.zeros([rowed, colD])

        ie = 0

        for i in range(rowed):
            C = np.matrix(
                [[1, ex[ie, 0], ey[ie, 0], 0, 0, 0],
                 [0, 0, 0, 1, ex[ie, 0], ey[ie, 0]],
                 [1, ex[ie, 1], ey[ie, 1], 0, 0, 0],
                 [0, 0, 0, 1, ex[ie, 1], ey[ie, 1]],
                 [1, ex[ie, 2], ey[ie, 2], 0, 0, 0],
                 [0, 0, 0, 1, ex[ie, 2], ey[ie, 2]]]
            )

            B = np.matrix([
                [0, 1, 0, 0, 0, 0],
                [0, 0, 0, 0, 0, 1],
                [0, 0, 1, 0, 1, 0]])*np.linalg.inv(C)

            ee = B*np.asmatrix(ed[ie, :]).T

            if colD > 3:
                ss = np.zeros([colD, 1])
                ss[[0, 1, 3]] = Dm*ee
                ee = Cm*ss
            else:
                ss = Dm*ee

            et[ie, :] = ee.T
            es[ie, :] = ss.T

            ie = ie + incie

        return es, et

    # --------- plane strain --------------------------------------
    elif ptype == 2:  # Implementation by LAPM
        colD = D.shape[1]
        incie = 0

        if rowex == 1:
            incie = 0
        else:
            incie = 1

        et = np.zeros([rowed, colD])
        es = np.zeros([rowed, colD])

        ie = 0

        ee = np.zeros([colD, 1])

        for i in range(rowed):
            C = np.matrix(
                [[1, ex[ie, 0], ey[ie, 0], 0, 0, 0],
                 [0, 0, 0, 1, ex[ie, 0], ey[ie, 0]],
                 [1, ex[ie, 1], ey[ie, 1], 0, 0, 0],
                 [0, 0, 0, 1, ex[ie, 1], ey[ie, 1]],
                 [1, ex[ie, 2], ey[ie, 2], 0, 0, 0],
                 [0, 0, 0, 1, ex[ie, 2], ey[ie, 2]]]
            )

            B = np.matrix([
                [0, 1, 0, 0, 0, 0],
                [0, 0, 0, 0, 0, 1],
                [0, 0, 1, 0, 1, 0]])*np.linalg.inv(C)

            e = B*np.asmatrix(ed[ie, :]).T

            if colD > 3:
                ee[[0, 1, 3]] = e
            else:
                ee = e

            et[ie, :] = ee.T
            es[ie, :] = (D*ee).T

            ie = ie + incie

        return es, et

    else:
        print("Error ! Check first argument, ptype=1 or 2 allowed")
        return None


def plantf(ex, ey, ep, es):
    """
    Compute internal element force vector in a triangular element
    in plane stress or plane strain. 

    Parameters:

        ex = [x1,x2,x3]                 node coordinates
        ey = [y1,y2,y3]

        ep = [ptype,t]                  ptype: analysis type
                                        t: thickness

        es = [[sigx,sigy,[sigz],tauxy]  element stress matrix
              [  ......              ]] one row for each element

    OUTPUT:

        fe = [[f1],[f2],...,[f8]]       internal force vector

    """

    ptype, t = ep

    colD = es.shape[1]

    #--------- plane stress --------------------------------------

    if ptype == 1:

        C = np.mat([
            [1, ex[0], ey[0], 0, 0,     0],
            [0, 0,     0,     1, ex[0], ey[0]],
            [1, ex[1], ey[1], 0, 0,     0],
            [0, 0,     0,     1, ex[1], ey[1]],
            [1, ex[2], ey[2], 0, 0,     0],
            [0, 0,     0,     1, ex[2], ey[2]]
        ])

        A = 0.5*np.linalg.det(np.mat([
            [1, ex[0], ey[0]],
            [1, ex[1], ey[1]],
            [1, ex[2], ey[2]]
        ]))

        B = np.mat([
            [0, 1, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 1],
            [0, 0, 1, 0, 1, 0]
        ])*np.linalg.inv(C)

        if colD > 3:
            stress = np.asmatrix(es[np.ix_((0, 1, 3))])
        else:
            stress = np.asmatrix(es)

        ef = (A*t*B.T*stress.T).T

        return np.reshape(np.asarray(ef), 6)

    #--------- plane strain --------------------------------------

    elif ptype == 2:

        C = np.mat([
            [1, ex[0], ey[0], 0, 0,     0],
            [0, 0,     0,     1, ex[0], ey[0]],
            [1, ex[1], ey[1], 0, 0,     0],
            [0, 0,     0,     1, ex[1], ey[1]],
            [1, ex[2], ey[2], 0, 0,     0],
            [0, 0,     0,     1, ex[2], ey[2]]
        ])

        A = 0.5*np.linalg.det(np.mat([
            [1, ex[0], ey[0]],
            [1, ex[1], ey[1]],
            [1, ex[2], ey[2]]
        ]))

        B = np.mat([
            [0, 1, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 1],
            [0, 0, 1, 0, 1, 0]
        ])*np.linalg.inv(C)

        if colD > 3:
            stress = np.asmatrix(es[np.ix_((0, 1, 3))])
        else:
            stress = np.asmatrix(es)

        ef = (A*t*B.T*stress.T).T

        return np.reshape(np.asarray(ef), 6)

    else:
        info("Error ! Check first argument, ptype=1 or 2 allowed")
        return None


def platre(ex, ey, ep, D, eq=None):
    """
    Calculate the stiffness matrix for a rectangular plate element.
    NOTE! Element sides must be parallel to the coordinate axis.
    
    Parameters:

        ex = [x1,x2,x3,x4]          element coordinates
        ey = [y1,y2,y3,y4]

        ep = [t]                    thicknes

        D                           constitutive matrix for
                                    plane stress         

        eq = [qz]                   load/unit area
    Returns:

        Ke                          element stiffness matrix (12 x 12)
        fe                          equivalent nodal forces (12 x 1)

    """
    Lx = (ex[2]-ex[0]).astype(float)
    Ly = (ey[2]-ey[0]).astype(float)
    t = ep[0]

    D = t**3/12.*D

    A1 = Ly/(Lx**3)
    A2 = Lx/(Ly**3)
    A3 = 1/Lx/Ly
    A4 = Ly/(Lx**2)
    A5 = Lx/(Ly**2)
    A6 = 1/Lx
    A7 = 1/Ly
    A8 = Ly/Lx
    A9 = Lx/Ly

    C1 = 4*A1*D[0, 0]+4*A2*D[1, 1]+2*A3*D[0, 1]+5.6*A3*D[2, 2]
    C2 = -4*A1*D[0, 0]+2*A2*D[1, 1]-2*A3*D[0, 1]-5.6*A3*D[2, 2]
    C3 = 2*A1*D[0, 0]-4*A2*D[1, 1]-2*A3*D[0, 1]-5.6*A3*D[2, 2]
    C4 = -2*A1*D[0, 0]-2*A2*D[1, 1]+2*A3*D[0, 1]+5.6*A3*D[2, 2]
    C5 = 2*A5*D[1, 1]+A6*D[0, 1]+0.4*A6*D[2, 2]
    C6 = 2*A4*D[0, 0]+A7*D[0, 1]+0.4*A7*D[2, 2]

    C7 = 2*A5*D[1, 1]+0.4*A6*D[2, 2]
    C8 = 2*A4*D[0, 0]+0.4*A7*D[2, 2]
    C9 = A5*D[1, 1]-A6*D[0, 1]-0.4*A6*D[2, 2]
    C10 = A4*D[0, 0]-A7*D[0, 1]-0.4*A7*D[2, 2]
    C11 = A5*D[1, 1]-0.4*A6*D[2, 2]
    C12 = A4*D[0, 0]-0.4*A7*D[2, 2]

    C13 = 4/3.*A9*D[1, 1]+8/15.*A8*D[2, 2]
    C14 = 4/3.*A8*D[0, 0]+8/15.*A9*D[2, 2]
    C15 = 2/3.*A9*D[1, 1]-8/15.*A8*D[2, 2]
    C16 = 2/3.*A8*D[0, 0]-8/15.*A9*D[2, 2]
    C17 = 2/3.*A9*D[1, 1]-2/15.*A8*D[2, 2]
    C18 = 2/3.*A8*D[0, 0]-2/15.*A9*D[2, 2]
    C19 = 1/3.*A9*D[1, 1]+2/15.*A8*D[2, 2]
    C20 = 1/3.*A8*D[0, 0]+2/15.*A9*D[2, 2]
    C21 = D[0, 1]

    Keq = np.mat(np.zeros((12, 12)))
    Keq[0, 0:13] = C1, C5, -C6, C2, C9, -C8, C4, C11, -C12, C3, C7, -C10
    Keq[1, 1:13] = C13, -C21, C9, C15, 0, -C11, C19, 0, -C7, C17, 0
    Keq[2, 2:13] = C14, C8, 0, C18, C12, 0, C20, -C10, 0, C16
    Keq[3, 3:13] = C1, C5, C6, C3, C7, C10, C4, C11, C12
    Keq[4, 4:13] = C13, C21, -C7, C17, 0, -C11, C19, 0
    Keq[5, 5:13] = C14, C10, 0, C16, -C12, 0, C20
    Keq[6, 6:13] = C1, -C5, C6, C2, -C9, C8
    Keq[7, 7:13] = C13, -C21, -C9, C15, 0
    Keq[8, 8:13] = C14, -C8, 0, C18
    Keq[9, 9:13] = C1, -C5, -C6
    Keq[10, 10:13] = C13, C21
    Keq[11, 11] = C14
    Keq = Keq.T+Keq-np.diag(np.diag(Keq))

    if eq != None:
        q = eq
        R1 = q*Lx*Ly/4
        R2 = q*Lx*Ly**2/24
        R3 = q*Ly*Lx**2/24

        feq = np.mat([R1, R2, -R3, R1, R2, R3, R1, -R2, R3, R1, -R2, -R3])

    if eq != None:
        return Keq, feq
    else:
        return Keq


def planqe(ex, ey, ep, D, eq=None):
    """
    Calculate the stiffness matrix for a quadrilateral
    plane stress or plane strain element.

    Parameters:
        ex=[x1 x2 x3 x4]    element coordinates
        ey=[y1 y2 y3 y4]
                                
        ep = [ptype, t]     ptype: analysis type
                            t: element thickness 

        D                   constitutive matrix

        eq = [bx;           bx: body force in x direction
              by]           by: body force in y direction

    OUTPUT: Ke :  element stiffness matrix (8 x 8)
            fe : equivalent nodal forces (row array)
    """
    K = np.zeros((10, 10))
    f = np.zeros((10, 1))

    xm = sum(ex)/4.
    ym = sum(ey)/4.

    b1 = eq if eq is not None else np.array([[0], [0]])

    ke1, fe1 = plante(np.array([ex[0], ex[1], xm]),
                      np.array([ey[0], ey[1], ym]), ep, D, b1)
    K, f = assem(np.array([1, 2, 3, 4, 9, 10]), K, ke1, f, fe1)
    ke1, fe1 = plante(np.array([ex[1], ex[2], xm]),
                      np.array([ey[1], ey[2], ym]), ep, D, b1)
    K, f = assem(np.array([3, 4, 5, 6, 9, 10]), K, ke1, f, fe1)
    ke1, fe1 = plante(np.array([ex[2], ex[3], xm]),
                      np.array([ey[2], ey[3], ym]), ep, D, b1)
    K, f = assem(np.array([5, 6, 7, 8, 9, 10]), K, ke1, f, fe1)
    ke1, fe1 = plante(np.array([ex[3], ex[0], xm]),
                      np.array([ey[3], ey[0], ym]), ep, D, b1)
    K, f = assem(np.array([7, 8, 1, 2, 9, 10]), K, ke1, f, fe1)
    Ke, fe = statcon(K, f, np.array([[9], [10]]))

    if eq is None:
        return Ke
    else:
        return Ke, fe


def planqs(ex, ey, ep, D, ed, eq=None):
    """
    Calculate element normal and shear stress for a quadrilateral 
    plane stress or plane strain element.
    
    Parameters:
            ex = [x1 x2 x3 x4]      element coordinates
            ey = [y1 y2 y3 y4]
    
            ep = [ptype, t]         ptype: analysis type
                                    t:  thickness
                                   
            D                       constitutive matrix
    
            ed = [u1 u2 ..u8]       element displacement vector
    
            eq = [[bx]               bx: body force in x direction
                  [by]]              by: body force in y direction
    
    OUTPUT: es = [ sigx sigy (sigz) tauxy]    element stress array
            et = [ epsx epsy (epsz) gamxy]    element strain array
    """

    if ex.shape != (4,) or ey.shape != (4,) or ed.shape != (8,):
        raise ValueError(
            'Error ! PLANQS: only one element at the time (ex, ey, ed must be a row arrays)')

    K = np.zeros((10, 10))
    f = np.zeros((10, 1))

    xm = sum(ex)/4.
    ym = sum(ey)/4.

    b1 = eq if eq is not None else np.array([[0], [0]])

    ex1 = np.array([ex[0], ex[1], xm])
    ey1 = np.array([ey[0], ey[1], ym])
    ex2 = np.array([ex[1], ex[2], xm])
    ey2 = np.array([ey[1], ey[2], ym])
    ex3 = np.array([ex[2], ex[3], xm])
    ey3 = np.array([ey[2], ey[3], ym])
    ex4 = np.array([ex[3], ex[0], xm])
    ey4 = np.array([ey[3], ey[0], ym])

    ke1, fe1 = plante(ex1, ey1, ep, D, b1)
    K, f = assem(np.array([1, 2, 3, 4, 9, 10]), K, ke1, f, fe1)
    ke1, fe1 = plante(ex2, ey2, ep, D, b1)
    K, f = assem(np.array([3, 4, 5, 6, 9, 10]), K, ke1, f, fe1)
    ke1, fe1 = plante(ex3, ey3, ep, D, b1)
    K, f = assem(np.array([5, 6, 7, 8, 9, 10]), K, ke1, f, fe1)
    ke1, fe1 = plante(ex4, ey4, ep, D, b1)
    K, f = assem(np.array([7, 8, 1, 2, 9, 10]), K, ke1, f, fe1)

    A1 = 0.5 * \
        np.linalg.det(
            np.hstack([np.ones((3, 1)), np.mat(ex1).T, np.mat(ey1).T]))
    A2 = 0.5 * \
        np.linalg.det(
            np.hstack([np.ones((3, 1)), np.mat(ex2).T, np.mat(ey2).T]))
    A3 = 0.5 * \
        np.linalg.det(
            np.hstack([np.ones((3, 1)), np.mat(ex3).T, np.mat(ey3).T]))
    A4 = 0.5 * \
        np.linalg.det(
            np.hstack([np.ones((3, 1)), np.mat(ex4).T, np.mat(ey4).T]))
    Atot = A1+A2+A3+A4

    a, _ = solveq(K, f, np.array(range(1, 9)), ed)

#    ni = ed.shape[0]
#    a = np.mat(empty((10,ni)))
#    for i in range(ni):
#        a[:,i] = solveq(K, f, np.array(range(1,9)), ed[i,:])[0]
#        #a = np.hstack([a, solveq(K, f, np.hstack([matrix(range(1,9)).T, ed[i,:].T]) ) ])

    s1, t1 = plants(ex1, ey1, ep, D, np.hstack([a[[0, 1, 2, 3, 8, 9], :].T]))
    s2, t2 = plants(ex2, ey2, ep, D, np.hstack([a[[2, 3, 4, 5, 8, 9], :].T]))
    s3, t3 = plants(ex3, ey3, ep, D, np.hstack([a[[4, 5, 6, 7, 8, 9], :].T]))
    s4, t4 = plants(ex4, ey4, ep, D, np.hstack([a[[6, 7, 0, 1, 8, 9], :].T]))

    es = (s1*A1+s2*A2+s3*A3+s4*A4)/Atot
    et = (t1*A1+t2*A2+t3*A3+t4*A4)/Atot

    # [0] because these are 1-by-3 arrays and we want row arrays out.
    return es[0], et[0]


def plani4e(ex, ey, ep, D, eq=None):
    """
    Calculate the stiffness matrix for a 4 node isoparametric
    element in plane strain or plane stress.
    
    Parameters:
        ex = [x1 ...   x4]  element coordinates. Row array
        ey = [y1 ...   y4]
                                
        ep =[ptype, t, ir]  ptype: analysis type
                            t : thickness
                            ir: integration rule
    
        D                   constitutive matrix
    
        eq = [bx; by]       bx: body force in x direction
                            by: body force in y direction
                                Any array with 2 elements acceptable
    
    Returns:
        Ke : element stiffness matrix (8 x 8)
        fe : equivalent nodal forces (8 x 1)
    """
    ptype = ep[0]
    t = ep[1]
    ir = ep[2]
    ngp = ir*ir
    if eq is None:
        q = np.zeros((2, 1))
    else:
        q = np.reshape(eq, (2, 1))
#--------- gauss points --------------------------------------
    if ir == 1:
        g1 = 0.0
        w1 = 2.0
        gp = np.mat([g1, g1])
        w = np.mat([w1, w1])
    elif ir == 2:
        g1 = 0.577350269189626
        w1 = 1
        gp = np.mat([
            [-g1, -g1],
            [g1, -g1],
            [-g1, g1],
            [g1, g1]])
        w = np.mat([
            [w1, w1],
            [w1, w1],
            [w1, w1],
            [w1, w1]])
    elif ir == 3:
        g1 = 0.774596669241483
        g2 = 0.
        w1 = 0.555555555555555
        w2 = 0.888888888888888
        gp = np.mat([
            [-g1, -g1],
            [-g2, -g1],
            [g1, -g1],
            [-g1, g2],
            [g2, g2],
            [g1, g2],
            [-g1, g1],
            [g2, g1],
            [g1, g1]])
        w = np.mat([
            [w1, w1],
            [w2, w1],
            [w1, w1],
            [w1, w2],
            [w2, w2],
            [w1, w2],
            [w1, w1],
            [w2, w1],
            [w1, w1]])
    else:
        info("Used number of integrat     ion points not implemented")
    wp = np.multiply(w[:, 0], w[:, 1])
    xsi = gp[:, 0]
    eta = gp[:, 1]
    r2 = ngp*2
    # Shape Functions
    N = np.multiply((1-xsi), (1-eta))/4.
    N = np.append(N, np.multiply((1+xsi), (1-eta))/4., axis=1)
    N = np.append(N, np.multiply((1+xsi), (1+eta))/4., axis=1)
    N = np.append(N, np.multiply((1-xsi), (1+eta))/4., axis=1)

    dNr = np.mat(np.zeros((r2, 4)))
    dNr[0:r2:2, 0] = -(1-eta)/4.
    dNr[0:r2:2, 1] = (1-eta)/4.
    dNr[0:r2:2, 2] = (1+eta)/4.
    dNr[0:r2:2, 3] = -(1+eta)/4.
    dNr[1:r2+1:2, 0] = -(1-xsi)/4.
    dNr[1:r2+1:2, 1] = -(1+xsi)/4.
    dNr[1:r2+1:2, 2] = (1+xsi)/4.
    dNr[1:r2+1:2, 3] = (1-xsi)/4.

#
    Ke1 = np.mat(np.zeros((8, 8)))
    fe1 = np.mat(np.zeros((8, 1)))
    JT = dNr*np.mat([ex, ey]).T
    # --------- plane stress --------------------------------------
    if ptype == 1:
        colD = np.shape(D)[0]
        if colD > 3:
            Cm = np.linalg.inv(D)
            Dm = np.linalg.inv(Cm[np.ix_([0, 1, 3], [0, 1, 3])])
        else:
            Dm = D
#
        B = np.matrix(np.zeros((3, 8)))
        N2 = np.matrix(np.zeros((2, 8)))
        for i in range(ngp):
            indx = np.array([2*(i+1)-1, 2*(i+1)])
            detJ = np.linalg.det(JT[indx-1, :])
            if detJ < 10*np.finfo(float).eps:
                info("Jacobi determinant equal or less than zero!")
            JTinv = np.linalg.inv(JT[indx-1, :])
            dNx = JTinv*dNr[indx-1, :]
#
            index_array_even = np.array([0, 2, 4, 6])
            index_array_odd = np.array([1, 3, 5, 7])
#
            counter = 0
            for index in index_array_even:
                B[0, index] = dNx[0, counter]
                B[2, index] = dNx[1, counter]
                N2[0, index] = N[i, counter]
                counter = counter+1
#
            counter = 0
            for index in index_array_odd:
                B[1, index] = dNx[1, counter]
                B[2, index] = dNx[0, counter]
                N2[1, index] = N[i, counter]
                counter = counter+1
#
            Ke1 = Ke1+B.T*Dm*B*detJ*wp[i].item()*t
            fe1 = fe1 + N2.T * q * detJ * wp[i].item() * t

        return Ke1, fe1
#--------- plane strain --------------------------------------
    elif ptype == 2:
        #
        colD = np.shape(D)[0]
        if colD > 3:
            Dm = D[np.ix_([0, 1, 3], [0, 1, 3])]
        else:
            Dm = D
#
        B = np.matrix(np.zeros((3, 8)))
        N2 = np.matrix(np.zeros((2, 8)))
        for i in range(ngp):
            indx = np.array([2*(i+1)-1, 2*(i+1)])
            detJ = np.linalg.det(JT[indx-1, :])
            if detJ < 10*np.finfo(float).eps:
                info("Jacobideterminant equal or less than zero!")
            JTinv = np.linalg.inv(JT[indx-1, :])
            dNx = JTinv*dNr[indx-1, :]
#
            index_array_even = np.array([0, 2, 4, 6])
            index_array_odd = np.array([1, 3, 5, 7])
#
            counter = 0
            for index in index_array_even:
                #
                B[0, index] = dNx[0, counter]
                B[2, index] = dNx[1, counter]
                N2[0, index] = N[i, counter]
#
                counter = counter+1
#
            counter = 0
            for index in index_array_odd:
                B[1, index] = dNx[1, counter]
                B[2, index] = dNx[0, counter]
                N2[1, index] = N[i, counter]
                counter = counter+1
#
            Ke1 = Ke1 + B.T * Dm * B * detJ * np.asscalar(wp[i]) * t
            fe1 = fe1+N2.T*q*detJ*np.asscalar(wp[i])*t
        return Ke1, fe1
    else:
        info("Error ! Check first argument, ptype=1 or 2 allowed")


def soli8e(ex, ey, ez, ep, D, eqp=None):
    """
    Ke=soli8e(ex,ey,ez,ep,D)
    [Ke,fe]=soli8e(ex,ey,ez,ep,D,eq)
    -------------------------------------------------------------
    PURPOSE
    Calculate the stiffness matrix for a 8 node (brick)
    isoparametric element.

    INPUT:   ex = [x1 x2 x3 ... x8]
            ey = [y1 y2 y3 ... y8]  element coordinates
            ez = [z1 z2 z3 ... z8]

            ep = [ir]               ir integration rule

            D                       constitutive matrix

            eq = [bx; by; bz]       bx: body force in x direction
                                    by: body force in y direction
                                    bz: body force in z direction

    OUTPUT: Ke : element stiffness matrix
            fe : equivalent nodal forces 
    -------------------------------------------------------------

    LAST MODIFIED: M Ristinmaa   1995-10-25
                   J Lindemann   2022-01-24 (Python version)
    Copyright (c)  Division of Structural Mechanics and
                   Division of Solid Mechanics.
                   Lund University
    -------------------------------------------------------------    
    """
    ir = ep[0]
    ngp = ir*ir*ir

    if eqp is None:
        eq = np.zeros((3, 1))
    else:
        eq = eqp

    if ir == 1:
        g1 = 0.0
        w1 = 2.0
        gp = np.array([g1, g1, g1]).reshape(1, 3)
        w = np.array([w1, w1, w1]).reshape(1, 3)
    elif ir == 2:
        g1 = 0.577350269189626
        w1 = 1
        gp = np.zeros((8, 3))
        w = np.zeros((8, 3))
        gp[:, 0] = np.array([-1, 1, 1, -1, -1, 1, 1, -1])*g1
        w[:, 0] = np.array([1, 1, 1, 1, 1, 1, 1, 1])*w1
        gp[:, 1] = np.array([-1, -1, 1, 1, -1, -1, 1, 1])*g1
        w[:, 1] = np.array([1, 1, 1, 1, 1, 1, 1, 1])*w1
        gp[:, 2] = np.array([-1, -1, -1, -1, 1, 1, 1, 1])*g1
        w[:, 2] = np.array([1, 1, 1, 1, 1, 1, 1, 1])*w1
    else:
        g1 = 0.774596669241483,
        g2 = 0.0
        w1 = 0.555555555555555
        w2 = 0.888888888888888

        gp = np.zeros((27, 3))
        w = np.zeros((27, 3))

        I1 = np.array([-1, 0, 1, -1, 0, 1, -1, 0, 1]).reshape(1, 9)
        I2 = np.array([0, -1, 0, 0, 1, 0, 0, 1, 0]).reshape(1, 9)

        gp[:, 0] = np.concatenate((I1, I1, I1), axis=1)*g1
        gp[:, 0] = np.concatenate((I2, I2, I2), axis=1)*g2 + gp[:, 0]

        I1 = np.abs(I1)
        I2 = np.abs(I2)

        w[:, 0] = np.concatenate((I1, I1, I1), axis=1)*w1
        w[:, 0] = np.concatenate((I2, I2, I2), axis=1)*w2 + w[:, 0]

        I1 = np.array([-1, -1, -1, 0, 0, 0, 1, 1, 1]).reshape(1, 9)
        I2 = np.array([0, 0, 0, 1, 1, 1, 0, 0, 0]).reshape(1, 9)

        gp[:, 1] = np.concatenate((I1, I1, I1), axis=1)*g1
        gp[:, 1] = np.concatenate((I2, I2, I2), axis=1)*g2 + gp[:, 1]

        I1 = np.abs(I1)
        I2 = np.abs(I2)

        w[:, 1] = np.concatenate((I1, I1, I1), axis=1)*w1
        w[:, 1] = np.concatenate((I2, I2, I2), axis=1)*w2 + w[:, 1]

        I1 = np.array([-1, -1, -1, -1, -1, -1, -1, -1, -1]).reshape(1, 9)
        I2 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]).reshape(1, 9)
        I3 = np.abs(I1)

        gp[:, 2] = np.concatenate((I1, I2, I3), axis=1)*g1
        gp[:, 2] = np.concatenate((I2, I3, I2), axis=1)*g2 + gp[:, 2]

        w[:, 2] = np.concatenate((I3, I2, I3), axis=1)*w1
        w[:, 2] = np.concatenate((I2, I3, I2), axis=1)*w2 + w[:, 2]

    wp = w[:, 0]*w[:, 1]*w[:, 2]

    xsi = gp[:, 0]
    eta = gp[:, 1]
    zet = gp[:, 2]
    r2 = ngp*3

    N = np.zeros((ngp, 8))
    dNr = np.zeros((r2, 8))

    N[:, 0] = (1-xsi)*(1-eta)*(1-zet)/8
    N[:, 1] = (1+xsi)*(1-eta)*(1-zet)/8
    N[:, 2] = (1+xsi)*(1+eta)*(1-zet)/8
    N[:, 3] = (1-xsi)*(1+eta)*(1-zet)/8
    N[:, 4] = (1-xsi)*(1-eta)*(1+zet)/8
    N[:, 5] = (1+xsi)*(1-eta)*(1+zet)/8
    N[:, 6] = (1+xsi)*(1+eta)*(1+zet)/8
    N[:, 7] = (1-xsi)*(1+eta)*(1+zet)/8

    dNr[0:r2+1:3, 0] = -(1-eta)*(1-zet)
    dNr[0:r2+1:3, 1] = (1-eta)*(1-zet)
    dNr[0:r2+1:3, 2] = (1+eta)*(1-zet)
    dNr[0:r2+1:3, 3] = -(1+eta)*(1-zet)
    dNr[0:r2+1:3, 4] = -(1-eta)*(1+zet)
    dNr[0:r2+1:3, 5] = (1-eta)*(1+zet)
    dNr[0:r2+1:3, 6] = (1+eta)*(1+zet)
    dNr[0:r2+1:3, 7] = -(1+eta)*(1+zet)
    dNr[1:r2+2:3, 0] = -(1-xsi)*(1-zet)
    dNr[1:r2+2:3, 1] = -(1+xsi)*(1-zet)
    dNr[1:r2+2:3, 2] = (1+xsi)*(1-zet)
    dNr[1:r2+2:3, 3] = (1-xsi)*(1-zet)
    dNr[1:r2+2:3, 4] = -(1-xsi)*(1+zet)
    dNr[1:r2+2:3, 5] = -(1+xsi)*(1+zet)
    dNr[1:r2+2:3, 6] = (1+xsi)*(1+zet)
    dNr[1:r2+2:3, 7] = (1-xsi)*(1+zet)
    dNr[2:r2+3:3, 0] = -(1-xsi)*(1-eta)
    dNr[2:r2+3:3, 1] = -(1+xsi)*(1-eta)
    dNr[2:r2+3:3, 2] = -(1+xsi)*(1+eta)
    dNr[2:r2+3:3, 3] = -(1-xsi)*(1+eta)
    dNr[2:r2+3:3, 4] = (1-xsi)*(1-eta)
    dNr[2:r2+3:3, 5] = (1+xsi)*(1-eta)
    dNr[2:r2+3:3, 6] = (1+xsi)*(1+eta)
    dNr[2:r2+3:3, 7] = (1-xsi)*(1+eta)

    dNr = dNr/8.0

    Ke = np.zeros((24, 24))
    fe = np.zeros((24, 1))

    ex = np.asarray(ex).reshape((8, 1))
    ey = np.asarray(ey).reshape((8, 1))
    ez = np.asarray(ez).reshape((8, 1))

    JT = dNr@np.concatenate((ex, ey, ez), axis=1)

    eps = np.finfo(float).eps

    for i in range(ngp):
        indx = [i*3, i*3+1, i*3+2]
        detJ = np.linalg.det(JT[indx, :])
        if detJ < 10*eps:
            print('Jacobideterminant equal or less than zero!')
        JTinv = np.linalg.inv(JT[indx, :])
        dNx = JTinv@dNr[indx, :]

        B = np.zeros((6, 24))
        N2 = np.zeros((3, 24))

        B[0, 0:24:3] = dNx[0, :]
        B[1, 1:25:3] = dNx[1, :]
        B[2, 2:26:3] = dNx[2, :]
        B[3, 0:24:3] = dNx[1, :]
        B[3, 1:25:3] = dNx[0, :]
        B[4, 0:24:3] = dNx[2, :]
        B[4, 2:26:3] = dNx[0, :]
        B[5, 1:25:3] = dNx[2, :]
        B[5, 2:26:3] = dNx[1, :]

        N2[0, 0:24:3] = N[i, :]
        N2[1, 1:25:3] = N[i, :]
        N2[2, 2:26:3] = N[i, :]

        Ke = Ke + (np.transpose(B)@D@B)*detJ*wp[i]
        fe = fe + (np.transpose(N2)@eq)*detJ*wp[i]

    if eqp != None:
        return Ke, fe
    else:
        return Ke


def soli8s(ex, ey, ez, ep, D, ed):
    """
     [es,et]=soli8s(ex,ey,ez,ep,D,ed)
    -------------------------------------------------------------
     PURPOSE
      Calculate element normal and shear stress for a
      8 node (brick) isoparametric element.
    
     INPUT:  ex = [x1 x2 x3 ... x8]
             ey = [y1 y2 y3 ... y8]  element coordinates
             ez = [z1 z2 z3 ... z8]
    
             ep = [Ir]               Ir: integration rule
    
             D                       constitutive matrix
    
             ed = [u1 u2 ..u24]      element displacement vector
      
     OUTPUT: es = [ sigx sigy sigz sigxy sigyz sigxz ;  
                      ......       ...               ] 
             element stress matrix, one row for each 
             integration point

             es = [ eps epsy epsz epsxy epsyz epsxz ;  
                      ......       ...               ] 
             element strain matrix, one row for each 
             integration point
    -------------------------------------------------------------
    
     LAST MODIFIED: M Ristinmaa   1995-10-25
                    J Lindemann   2022-02-23 (Python version)
                    
     Copyright (c)  Division of Structural Mechanics and
                    Division of Solid Mechanics.
                    Lund University
    -------------------------------------------------------------
    """

    ir = ep[0]
    ngp = ir*ir*ir

    ir = ep[0]
    ngp = ir*ir*ir

    if ir == 1:
        g1 = 0.0
        w1 = 2.0
        gp = np.array([g1, g1, g1]).reshape(1, 3)
        w = np.array([w1, w1, w1]).reshape(1, 3)
    elif ir == 2:
        g1 = 0.577350269189626
        w1 = 1
        gp = np.zeros((8, 3))
        w = np.zeros((8, 3))
        gp[:, 0] = np.array([-1, 1, 1, -1, -1, 1, 1, -1])*g1
        w[:, 0] = np.array([1, 1, 1, 1, 1, 1, 1, 1])*w1
        gp[:, 1] = np.array([-1, -1, 1, 1, -1, -1, 1, 1])*g1
        w[:, 1] = np.array([1, 1, 1, 1, 1, 1, 1, 1])*w1
        gp[:, 2] = np.array([-1, -1, -1, -1, 1, 1, 1, 1])*g1
        w[:, 2] = np.array([1, 1, 1, 1, 1, 1, 1, 1])*w1
    else:
        g1 = 0.774596669241483,
        g2 = 0.0
        w1 = 0.555555555555555
        w2 = 0.888888888888888

        gp = np.zeros((27, 3))
        w = np.zeros((27, 3))

        I1 = np.array([-1, 0, 1, -1, 0, 1, -1, 0, 1]).reshape(1, 9)
        I2 = np.array([0, -1, 0, 0, 1, 0, 0, 1, 0]).reshape(1, 9)

        gp[:, 0] = np.concatenate((I1, I1, I1), axis=1)*g1
        gp[:, 0] = np.concatenate((I2, I2, I2), axis=1)*g2 + gp[:, 0]

        I1 = np.abs(I1)
        I2 = np.abs(I2)

        w[:, 0] = np.concatenate((I1, I1, I1), axis=1)*w1
        w[:, 0] = np.concatenate((I2, I2, I2), axis=1)*w2 + w[:, 0]

        I1 = np.array([-1, -1, -1, 0, 0, 0, 1, 1, 1]).reshape(1, 9)
        I2 = np.array([0, 0, 0, 1, 1, 1, 0, 0, 0]).reshape(1, 9)

        gp[:, 1] = np.concatenate((I1, I1, I1), axis=1)*g1
        gp[:, 1] = np.concatenate((I2, I2, I2), axis=1)*g2 + gp[:, 1]

        I1 = np.abs(I1)
        I2 = np.abs(I2)

        w[:, 1] = np.concatenate((I1, I1, I1), axis=1)*w1
        w[:, 1] = np.concatenate((I2, I2, I2), axis=1)*w2 + w[:, 1]

        I1 = np.array([-1, -1, -1, -1, -1, -1, -1, -1, -1]).reshape(1, 9)
        I2 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]).reshape(1, 9)
        I3 = np.abs(I1)

        gp[:, 2] = np.concatenate((I1, I2, I3), axis=1)*g1
        gp[:, 2] = np.concatenate((I2, I3, I2), axis=1)*g2 + gp[:, 2]

        w[:, 2] = np.concatenate((I3, I2, I3), axis=1)*w1
        w[:, 2] = np.concatenate((I2, I3, I2), axis=1)*w2 + w[:, 2]

    wp = w[:, 0]*w[:, 1]*w[:, 2]

    xsi = gp[:, 0]
    eta = gp[:, 1]
    zet = gp[:, 2]
    r2 = ngp*3

    N = np.zeros((ngp, 8))
    dNr = np.zeros((r2, 8))

    N[:, 0] = (1-xsi)*(1-eta)*(1-zet)/8
    N[:, 1] = (1+xsi)*(1-eta)*(1-zet)/8
    N[:, 2] = (1+xsi)*(1+eta)*(1-zet)/8
    N[:, 3] = (1-xsi)*(1+eta)*(1-zet)/8
    N[:, 4] = (1-xsi)*(1-eta)*(1+zet)/8
    N[:, 5] = (1+xsi)*(1-eta)*(1+zet)/8
    N[:, 6] = (1+xsi)*(1+eta)*(1+zet)/8
    N[:, 7] = (1-xsi)*(1+eta)*(1+zet)/8

    dNr[0:r2+1:3, 0] = -(1-eta)*(1-zet)
    dNr[0:r2+1:3, 1] = (1-eta)*(1-zet)
    dNr[0:r2+1:3, 2] = (1+eta)*(1-zet)
    dNr[0:r2+1:3, 3] = -(1+eta)*(1-zet)
    dNr[0:r2+1:3, 4] = -(1-eta)*(1+zet)
    dNr[0:r2+1:3, 5] = (1-eta)*(1+zet)
    dNr[0:r2+1:3, 6] = (1+eta)*(1+zet)
    dNr[0:r2+1:3, 7] = -(1+eta)*(1+zet)
    dNr[1:r2+2:3, 0] = -(1-xsi)*(1-zet)
    dNr[1:r2+2:3, 1] = -(1+xsi)*(1-zet)
    dNr[1:r2+2:3, 2] = (1+xsi)*(1-zet)
    dNr[1:r2+2:3, 3] = (1-xsi)*(1-zet)
    dNr[1:r2+2:3, 4] = -(1-xsi)*(1+zet)
    dNr[1:r2+2:3, 5] = -(1+xsi)*(1+zet)
    dNr[1:r2+2:3, 6] = (1+xsi)*(1+zet)
    dNr[1:r2+2:3, 7] = (1-xsi)*(1+zet)
    dNr[2:r2+3:3, 0] = -(1-xsi)*(1-eta)
    dNr[2:r2+3:3, 1] = -(1+xsi)*(1-eta)
    dNr[2:r2+3:3, 2] = -(1+xsi)*(1+eta)
    dNr[2:r2+3:3, 3] = -(1-xsi)*(1+eta)
    dNr[2:r2+3:3, 4] = (1-xsi)*(1-eta)
    dNr[2:r2+3:3, 5] = (1+xsi)*(1-eta)
    dNr[2:r2+3:3, 6] = (1+xsi)*(1+eta)
    dNr[2:r2+3:3, 7] = (1-xsi)*(1+eta)

    dNr = dNr/8.0

    ex = np.asarray(ex).reshape((8, 1))
    ey = np.asarray(ey).reshape((8, 1))
    ez = np.asarray(ez).reshape((8, 1))

    JT = dNr@np.concatenate((ex, ey, ez), axis=1)

    eps = np.finfo(float).eps
    
    eci = N@np.concatenate((ex, ey, ez), axis=1)
    et = np.zeros((ngp, 6))
    es = np.zeros((ngp, 6))

    ed = ed.reshape(1, 24)

    for i in range(ngp):
        indx = [i*3, i*3+1, i*3+2]
        detJ = np.linalg.det(JT[indx, :])
        if detJ < 10*eps:
            print('Jacobideterminant equal or less than zero!')
        JTinv = np.linalg.inv(JT[indx, :])
        dNx = JTinv@dNr[indx, :]

        B = np.zeros((6, 24))
        N2 = np.zeros((3, 24))

        B[0, 0:24:3] = dNx[0, :]
        B[1, 1:25:3] = dNx[1, :]
        B[2, 2:26:3] = dNx[2, :]
        B[3, 0:24:3] = dNx[1, :]
        B[3, 1:25:3] = dNx[0, :]
        B[4, 0:24:3] = dNx[2, :]
        B[4, 2:26:3] = dNx[0, :]
        B[5, 1:25:3] = dNx[2, :]
        B[5, 2:26:3] = dNx[1, :]

        N2[0, 0:24:3] = N[i, :]
        N2[1, 1:25:3] = N[i, :]
        N2[2, 2:26:3] = N[i, :]

        # [6x24] x [24,1]
        ee = B@np.transpose(ed)

        et[i, :] = ee.reshape(6,)
        es[i, :] = (D@ee).reshape(6,)

    return et, es, eci


def assem(edof, K, Ke, f=None, fe=None):
    """
    Assemble element matrices Ke ( and fe ) into the global
    stiffness matrix K ( and the global force vector f )
    according to the topology matrix edof.
    
    Parameters:
    
        edof        dof topology array
        K           the global stiffness matrix
        Ke          element stiffness matrix
        f           the global force vector
        fe          element force vector
        
    Output parameters:
    
        K           the new global stiffness matrix
        f           the new global force vector
        fe          element force vector
    
    """

    if edof.ndim == 1:
        idx = edof-1
        K[np.ix_(idx, idx)] = K[np.ix_(idx, idx)] + Ke
        if (not f is None) and (not fe is None):
            f[np.ix_(idx)] = f[np.ix_(idx)] + fe
    else:
        for row in edof:
            idx = row-1
            K[np.ix_(idx, idx)] = K[np.ix_(idx, idx)] + Ke
            if (not f is None) and (not fe is None):
                f[np.ix_(idx)] = f[np.ix_(idx)] + fe

    if f is None:
        return K
    else:
        return K, f


def solveq(K, f, bcPrescr, bcVal=None):
    """
    Solve static FE-equations considering boundary conditions.
    
    Parameters:
    
        K           global stiffness matrix, dim(K)= nd x nd
        f           global load vector, dim(f)= nd x 1
    
        bcPrescr    1-dim integer array containing prescribed dofs.
        bcVal       1-dim float array containing prescribed values.
                    If not given all prescribed dofs are assumed 0.
        
    Returns:
    
        a           solution including boundary values
        Q           reaction force vector
                    dim(a)=dim(Q)= nd x 1, nd : number of dof's
    
    """

    nDofs = K.shape[0]
    nPdofs = bcPrescr.shape[0]

    if bcVal is None:
        bcVal = np.zeros([nPdofs], 'd')

    bc = np.ones(nDofs, 'bool')
    bcDofs = np.arange(nDofs)

    bc[np.ix_(bcPrescr-1)] = False
    bcDofs = bcDofs[bc]

    fsys = f[bcDofs]-K[np.ix_((bcDofs), (bcPrescr-1))] * \
        np.asmatrix(bcVal).reshape(nPdofs, 1)
    asys = np.linalg.solve(K[np.ix_((bcDofs), (bcDofs))], fsys)

    a = np.zeros([nDofs, 1])
    a[np.ix_(bcPrescr-1)] = np.asmatrix(bcVal).reshape(nPdofs, 1)
    a[np.ix_(bcDofs)] = asys

    Q = K*np.asmatrix(a)-f

    return (np.asmatrix(a), Q)


def spsolveq(K, f, bcPrescr, bcVal=None):
    """
    Solve static FE-equations considering boundary conditions.
    
    Parameters:
    
        K           global stiffness matrix, dim(K)= nd x nd
        f           global load vector, dim(f)= nd x 1
    
        bcPrescr    1-dim integer array containing prescribed dofs.
        bcVal       1-dim float array containing prescribed values.
                    If not given all prescribed dofs are assumed 0.
        
    Returns:
    
        a           solution including boundary values
        Q           reaction force vector
                    dim(a)=dim(Q)= nd x 1, nd : number of dof's
    
    """

    nDofs = K.shape[0]
    nPdofs = bcPrescr.shape[0]

    if bcVal is None:
        bcVal = np.zeros([nPdofs], 'd')

    bc = np.ones(nDofs, 'bool')
    bcDofs = np.arange(nDofs)

    bc[np.ix_(bcPrescr-1)] = False
    bcDofs = bcDofs[bc]

    bcVal_m = np.asmatrix(bcVal).reshape(nPdofs, 1)

    info("Preparing system matrix...")

    mask = np.ones(K.shape[0], dtype=bool)
    mask[bcDofs] = False

    info("step 1... converting K->CSR")
    Kcsr = K.asformat("csr")
    info("step 2... Kt")
    #Kt1 = K[bcDofs]
    #Kt = Kt1[:,bcPrescr]
    Kt = K[np.ix_((bcDofs), (bcPrescr-1))]
    info("step 3... fsys")
    fsys = f[bcDofs]-Kt*bcVal_m
    info("step 4... Ksys")
    Ksys1 = Kcsr[bcDofs]
    Ksys = Ksys1[:, bcDofs]
    #Ksys = Kcsr[np.ix_((bcDofs),(bcDofs))]
    info("done...")

    info("Solving system...")
    asys = dsolve.spsolve(Ksys, fsys)

    info("Reconstructing full a...")
    a = np.zeros([nDofs, 1])
    a[np.ix_(bcPrescr-1)] = bcVal_m
    a[np.ix_(bcDofs)] = np.asmatrix(asys).transpose()

    a_m = np.asmatrix(a)
    Q = K*a_m-f
    info("done...")
    return (a_m, Q)


def eigen(K,M,b=None):
    """
    Solve the generalized eigenvalue problem
    |K-LM|X = 0, considering boundary conditions

    Parameters:

        K           global stiffness matrix, dim(K) = ndof x ndof
        M           global mass matrix, dim(M) = ndof x ndof
        b           boundary condition vector, dim(b) = nbc x 1

    Returns:

        L           eigenvalue vector, dim(L) = (ndof-nbc) x 1
        X           eigenvectors, dim(X) = ndof x (ndof-nbc)
    """
    nd, _ = K.shape
    if b is not None:
        fdof = np.setdiff1d(np.arange(nd), b-1)
        D, X1 = eig(K[np.ix_(fdof,fdof)], M[np.ix_(fdof,fdof)])
        D = np.real(D)
        nfdof, _ = X1.shape
        for j in range(nfdof):
            mnorm = np.sqrt(X1[:,j].T@M[np.ix_(fdof,fdof)]@X1[:,j])
            X1[:,j] /= mnorm
        s_order = np.argsort(D)
        L = np.sort(D)
        X2 = np.zeros(X1.shape)
        for ind,j in enumerate(s_order):
            X2[:,ind] = X1[:,j]
        X = np.zeros((nd,nfdof))
        X[fdof,:] = X2
        return L, X
    else:
        D, X1 = eig(K, M)
        D = np.real(D)
        for j in range(nd):
            mnorm = np.sqrt(X1[:,j].T@M@X1[:,j])
            X1[:,j] /= mnorm
        s_order = np.argsort(D)
        L = np.sort(D)
        X = np.copy(X1)
        for ind,j in enumerate(s_order):
            X[:,ind] = X1[:,j]
        return L, X


def extract_eldisp(edof, a):
    """
    Extract element displacements from the global displacement
    vector according to the topology matrix edof.
    
    Parameters:
    
        a           the global displacement vector
        edof        dof topology array
    
    Returns:
    
        ed:     element displacement array
    
    """

    ed = None

    if edof.ndim == 1:
        nDofs = len(edof)
        ed = np.zeros([nDofs])
        idx = edof-1
        ed[:] = a[np.ix_(idx)].T
    else:
        nElements = edof.shape[0]
        nDofs = edof.shape[1]
        ed = np.zeros([nElements, nDofs])
        i = 0
        for row in edof:
            idx = row-1
            ed[i, :] = a[np.ix_(idx)].T
            i += 1

    return ed


extractEldisp = extract_eldisp
extract_ed = extract_eldisp


def statcon(K, f, cd):
    """
    Condensation of static FE-equations according to the vector cd.

    Parameters:
    
        K                       global stiffness matrix, dim(K) = nd x nd
        f                       global load vector, dim(f)= nd x 1

        cd                      vector containing dof's to be eliminated
                                dim(cd)= nc x 1, nc: number of condensed dof's
    Returns:
    
        K1                      condensed stiffness matrix,
                                dim(K1)= (nd-nc) x (nd-nc)
        f1                      condensed load vector, dim(f1)= (nd-nc) x 1
    """
    nd, nd = np.shape(K)
    cd = (cd-1).flatten()

    aindx = np.arange(nd)
    aindx = np.delete(aindx, cd, 0)
    bindx = cd

    Kaa = np.mat(K[np.ix_(aindx, aindx)])
    Kab = np.mat(K[np.ix_(aindx, bindx)])
    Kbb = np.mat(K[np.ix_(bindx, bindx)])

    fa = np.mat(f[aindx])
    fb = np.mat(f[bindx])

    K1 = Kaa-Kab*Kbb.I*Kab.T
    f1 = fa-Kab*Kbb.I*fb

    return K1, f1


def c_mul(a, b):
    return eval(hex((np.long(a) * b) & 0xFFFFFFFF)[:-1])


def dofHash(dof):
    if len(dof) == 1:
        return dof[0]
    value = 0x345678
    for item in dof:
        value = c_mul(1000003, value) ^ hash(item)
    value = value ^ len(dof)
    if value == -1:
        value = -2
    return value


def create_dofs(nCoords, nDof):
    """
    Create dof array [nCoords x nDof]
    """
    return np.arange(nCoords*nDof).reshape(nCoords, nDof)+1


createdofs = create_dofs


def coordxtr(edof, coords, dofs, nen=-1):
    """
    Create element coordinate matrices ex, ey, ez from edof
    coord and dofs matrices.
    
    Parameters:
    
        edof            [nel x (nen * nnd)], nnd = number of node dofs
        coords          [ncoords x ndims],   ndims = node dimensions
        dofs            [ncoords x nnd]
        
    Returns:
    
        ex              if ndims = 1
        ex, ey          if ndims = 2
        ex, ey, ez      if ndims = 3
    """

    # Create dictionary with dof indices

    dofDict = {}
    nDofs = np.size(dofs, 1)
    nElements = np.size(edof, 0)
    n_element_dofs = np.size(edof, 1)
    nDimensions = np.size(coords, 1)
    nElementDofs = np.size(edof, 1)

    if nen == -1:
        nElementNodes = int(nElementDofs/nDofs)
    else:
        nElementNodes = nen

    if nElementNodes*nDofs != n_element_dofs:
        nDofs = nElementNodes*nDofs - n_element_dofs
        user_warning(
            "dofs/edof mismatch. Using %d dofs per node when indexing." % nDofs)

    idx = 0
    for dof in dofs:
        #dofDict[dofHash(dof)] = idx
        dofDict[hash(tuple(dof[0:nDofs]))] = idx
        idx += 1

    # Loop over edof and extract element coords

    ex = np.zeros((nElements, nElementNodes))
    ey = np.zeros((nElements, nElementNodes))
    ez = np.zeros((nElements, nElementNodes))

    elementIdx = 0
    for etopo in edof:
        for i in range(nElementNodes):
            i0 = i*nDofs
            i1 = i*nDofs+nDofs-1
            dof = []
            if i0 == i1:
                dof = [etopo[i*nDofs]]
            else:
                dof = etopo[i*nDofs:(i*nDofs+nDofs)]

            nodeCoord = coords[dofDict[hash(tuple(dof[0:nDofs]))]]

            if nDimensions >= 1:
                ex[elementIdx, i] = nodeCoord[0]
            if nDimensions >= 2:
                ey[elementIdx, i] = nodeCoord[1]
            if nDimensions >= 3:
                ez[elementIdx, i] = nodeCoord[2]

        elementIdx += 1

    if nDimensions == 1:
        return ex

    if nDimensions == 2:
        return ex, ey

    if nDimensions == 3:
        return ex, ey, ez


coord_extract = coordxtr


def hooke(ptype, E, v):
    """
    Calculate the material matrix for a linear
    elastic and isotropic material.
    
    Parameters:
    
        ptype=  1:  plane stress
                2:  plane strain
                3:  axisymmetry
                4:  three dimensional
    
        E           Young's modulus
        v           Poissons const.
        
    Returns:
    
        D           material matrix
    
    """

    if ptype == 1:
        D = E*np.matrix(
            [[1, v, 0],
             [v, 1, 0],
             [0, 0, (1-v)/2]]
        )/(1-v**2)
    elif ptype == 2:
        D = E/(1+v)*np.matrix(
            [[1-v, v, v, 0],
             [v, 1-v, v, 0],
             [v, v, 1-v, 0],
             [0, 0, 0, (1-2*v)/2]]
        )/(1-2*v)
    elif ptype == 3:
        D = E/(1+v)*np.matrix(
            [[1-v, v, v, 0],
             [v, 1-v, v, 0],
             [v, v, 1-v, 0],
             [0, 0, 0, (1-2*v)/2]]
        )/(1-2*v)
    elif ptype == 4:
        D = E*np.matrix(
            [[1-v, v, v, 0, 0, 0],
             [v, 1-v, v, 0, 0, 0],
             [v, v, 1-v, 0, 0, 0],
             [0, 0, 0, (1-2*v)/2, 0, 0],
             [0, 0, 0, 0, (1-2*v)/2, 0],
             [0, 0, 0, 0, 0, (1-2*v)/2]]
        )/(1+v)/(1-2*v)
    else:
        info("ptype not supported.")

    return D


def effmises(es, ptype):
    """
    Calculate effective von mises stresses.
    
    Parameters:
        
        es
    
        ptype=  1:  plane stress
                2:  plane strain
                3:  axisymmetry
                4:  three dimensional
    
       es = [[sigx,sigy,[sigz],tauxy]  element stress matrix
              [  ......              ]] one row for each element
              
    Returns:
    
        eseff  = [eseff_0 .. eseff_nel-1]
    
    """

    nel = np.size(es, 0)
    escomps = np.size(es, 1)

    eseff = np.zeros([nel])

    if ptype == 1:
        sigxx = es[:, 0]
        sigyy = es[:, 1]
        sigxy = es[:, 2]
        eseff = np.sqrt(sigxx*sigxx+sigyy*sigyy-sigxx*sigyy+3*sigxy*sigxy)
        return eseff


def stress2nodal(eseff, edof):
    """
    Convert element effective stresses to nodal effective
    stresses.
    
    Parameters:
        
        eseff  = [eseff_0 .. eseff_nel-1] 
        edof   = [dof topology array]
    
    Returns:
    
        ev:     element value array [[ev_0_0 ev_0_1 ev_0_nen-1 ]
                                      ..
                                      ev_nel-1_0 ev_nel-1_1 ev_nel-1_nen-1]
                      
    """

    values = np.zeros(edof.max())
    elnodes = int(np.size(edof, 1) / 2)

    for etopo, eleseff in zip(edof, eseff):
        values[etopo-1] = values[etopo-1] + eleseff / elnodes

    evtemp = extractEldisp(edof, values)
    ev = evtemp[:, range(0, elnodes*2, 2)]

    return ev


def beam2crd_old(ex, ey, ed, mag):
    """
    -------------------------------------------------------------
        PURPOSE
            Calculate the element continous displacements for a 
            number of identical 2D Bernoulli beam elements. 
      
        INPUT:  ex,ey,
                ed,
                mag 
    
        OUTPUT: excd,eycd 
    -------------------------------------------------------------

     LAST MODIFIED: P-E AUSTRELL 1993-10-15
                    J Lindemann 2021-12-30 (Python)

     Copyright (c)  Division of Structural Mechanics and
                    Division of Solid Mechanics.
                    Lund University
    -------------------------------------------------------------
    """
    nie, ned = ed.shape

    excd = np.zeros([nie, 20])
    eycd = np.zeros([nie, 20])

    for i in range(nie):

        b = np.array([ex[i, 1]-ex[i, 0], ey[i, 1]-ey[i, 0]])
        L = np.asscalar(np.sqrt(b@np.transpose(b)))
        n = b/L

        G = np.array([
            [n[0], n[1],  0,    0,    0,   0],
            [-n[1], n[0],  0,    0,    0,   0],
            [0,    0,     1,    0,    0,   0],
            [0,    0,     0,   n[0], n[1], 0],
            [0,    0,     0,  -n[1], n[0], 0],
            [0,    0,     0,    0,    0,   1]
        ])

        d = ed[i, :]
        dl = G @ d

        xl = np.linspace(0.0, L, 20)
        one = np.ones(xl.shape)

        Cis = np.array([
            [-1.0, 1.0],
            [L, 0.0]
        ]) / L

        ds = np.array([dl[0], dl[3]]).reshape(2, 1)

        xl_one = np.transpose(np.vstack((xl, one)))

        ul = np.transpose(xl_one@Cis@ds)  # [20x1][2]

        Cib = np.array([
            [12,      6*L,  -12,    6*L],
            [-6*L, -4*L**2,  6*L, -2*L**2],
            [0,      L**3,    0,     0],
            [L**3,      0,     0,     0]
        ])/L**3

        db = np.array([dl[1], dl[2], dl[4], dl[5]]).reshape(4, 1)
        vl = np.transpose(np.transpose(
            np.vstack((xl**3/6, xl**2/2, xl, one)))@Cib@db)

        cld = np.vstack((ul, vl))
        A = np.array([
            [n[0], -n[1]],
            [n[1], n[0]]
        ])
        cd = A@cld

        # [2,1] x [1,20] + [2 x 1] x [1 x 20]
        #    [2 x 20]    +      [2 x 20]

        AA = A[:, 0].reshape(2, 1)
        XL = xl.reshape(1, 20)
        xyc = AA@XL + np.array([[ex[i, 0]], [ey[i, 0]]])@one.reshape(1, 20)

        excd[i, :] = xyc[0, :]+mag*cd[0, :]
        eycd[i, :] = xyc[1, :]+mag*cd[1, :]

    return excd, eycd


def beam2crd(ex=None, ey=None, ed=None, mag=None):
    """
    -------------------------------------------------------------
        PURPOSE
         Calculate the element continous displacements for a
         number of identical 2D Bernoulli beam elements.
    
        INPUT:  ex,ey,
                ed,
                mag
    
        OUTPUT: excd,eycd
    -------------------------------------------------------------

    LAST MODIFIED: P-E AUSTRELL 1993-10-15
    Copyright (c)  Division of Structural Mechanics and
                    Division of Solid Mechanics.
                    Lund University
    -------------------------------------------------------------
    """

    nie, ned = ed.shape

    n_coords = 21

    excd = np.zeros([nie, n_coords])
    eycd = np.zeros([nie, n_coords])

    for i in range(nie):
        b = np.array([ex[i, 1] - ex[i, 0], ey[i, 1] - ey[i, 0]])
        L = np.sqrt(b @ np.transpose(b))
        n = b / L

        G = np.array([
            [n[0], n[1], 0, 0, 0, 0],
            [-n[1], n[0], 0, 0, 0, 0],
            [0, 0, 1, 0, 0, 0],
            [0, 0, 0, n[0], n[1], 0],
            [0, 0, 0, - n[1], n[0], 0],
            [0, 0, 0, 0, 0, 1]
        ])

        d = np.transpose(ed[i, :])
        dl = G @ d
        xl = np.transpose(np.linspace(0, L, n_coords))
        one = np.ones(xl.shape)

        Cis = np.array([
            [-1, 1],
            [L, 0]
        ]) / L

        ds = np.array([dl[0], dl[3]]).reshape(2, 1)
        xl_one = np.transpose(np.vstack((xl, one)))
        ul = np.transpose(xl_one@Cis@ds)  # [20x1][2]

        Cib = np.array([
            [12, 6 * L, - 12, 6 * L],
            [- 6 * L, - 4 * L ** 2, 6 * L, - 2 * L ** 2],
            [0, L ** 3, 0, 0],
            [L ** 3, 0, 0, 0]
        ]) / L ** 3

        db = np.array([dl[1], dl[2], dl[4], dl[5]]).reshape(4, 1)
        vl = np.transpose(np.transpose(
            np.vstack((xl**3/6, xl**2/2, xl, one)))@Cib@db)

        cld = np.vstack((ul, vl))
        A = np.array([
            [n[0], -n[1]],
            [n[1], n[0]]
        ])
        cd = A@cld

        # [2,1] x [1,20] + [2 x 1] x [1 x 20]
        #    [2 x 20]    +      [2 x 20]

        AA = A[:, 0].reshape(2, 1)
        XL = xl.reshape(1, n_coords)
        xyc = AA@XL + np.array([[ex[i, 0]], [ey[i, 0]]]
                               )@one.reshape(1, n_coords)

        excd[i, :] = xyc[0, :]+mag*cd[0, :]
        eycd[i, :] = xyc[1, :]+mag*cd[1, :]

    return excd, eycd