stiffness Function

function stiffness(s, q, X, D) result(K)
  !! Stiffness matrix K = sum_g B^T D B det(J) w_g, dofs ordered node by node
  ! not pure: stdlib inv and det are not
  type(shapefunc), intent(in) :: s
  type(cubature), intent(in) :: q
  real(dp), intent(in) :: X(:,:)   !! nodal coordinates (dim, nnodes)
  real(dp), intent(in) :: D(:,:)   !! elasticity matrix from `material`
  real(dp) :: K(s%dim*s%nnodes, s%dim*s%nnodes)
  real(dp) :: jac(s%dim, s%dim), dN(s%dim, s%nnodes), B(size(D,1), s%dim*s%nnodes)
  integer :: g, n, i

  K = 0
  do g = 1, q%npoints
    jac = matmul(X, transpose(s%derv(:,:,g)))        ! dx/dxi
    dN = matmul(transpose(inv(jac)), s%derv(:,:,g))  ! dN/dx
    B = 0
    do n = 1, s%nnodes
      i = s%dim*(n - 1)
      select case (s%dim)
      case (1)
        B(1, i+1) = dN(1,n)
      case (2)
        B(1, i+1) = dN(1,n)
        B(2, i+2) = dN(2,n)
        B(3, i+1) = dN(2,n); B(3, i+2) = dN(1,n)
      case (3)
        B(1, i+1) = dN(1,n)
        B(2, i+2) = dN(2,n)
        B(3, i+3) = dN(3,n)
        B(4, i+1) = dN(2,n); B(4, i+2) = dN(1,n)
        B(5, i+1) = dN(3,n); B(5, i+3) = dN(1,n)
        B(6, i+2) = dN(3,n); B(6, i+3) = dN(2,n)
      end select
    end do
    K = K + matmul(transpose(B), matmul(D, B))*det(jac)*q%weights(g)
  end do
end function stiffness