      IMPLICIT REAL*8 (a-h, o-z), INTEGER*4 (i-n)

!     test program, don't worry about space waste

      DIMENSION coor(3,4),imate(1),rmate(2),stiff(48,48),ifix(9),              &
                scales(48),ips(48),f(48),x(48),                                &
                z(48,48),w(48),v1(48),v2(48)

!     unit tetra

      DATA coor /0.d0,0.d0,0.d0, 1.d0,0.d0,0.d0, 0.d0,1.d0,0.d0, 0.d0,0.d0,1.d0/
      DATA ifix /1,5,9,17,21,25,33,37,41/

      OPEN (UNIT=10,FILE='resu3.txt',STATUS='UNKNOWN')

!     general data

      imate(1) = 1
      rmate(1) = 1000.
      rmate(2) = 0.3
      ivers = 1
      npi = 11

   10 WRITE (10,*) ' npi = ',npi
      WRITE (10,*) ' -------------------------------------------------------'
      CALL FElib_Elem103_Stiff (coor,imate,rmate,ivers,npi,stiff)
      WRITE (10,*) ' Diagonal of stiff'
      WRITE (10,'(4e15.6)') (stiff(i,i),i=1,48)
      DO i = 2,48
         DO j = 1,i-1
            stiff(i,j) = stiff(j,i)
         ENDDO
      ENDDO

!     compute eigenvalues and vectors

      matz = 1
      CALL rs(48,48,stiff,w,matz,z,v1,v2,ierr)   ! routine of eigenval.f
      IF (ierr.NE.0) THEN
         WRITE (10,*) ' ierr=',ierr
         STOP
      ENDIF
      WRITE (10,*) ' Eigenvalues'
      WRITE (10,'(6f15.5)') (w(i),i=1,48)

!     eigenvectors

!!      DO ii = 7,48
!!            WRITE (10,20) ii
!!   20       FORMAT (/' Normalized Eigenvector ',i3)
!!            WRITE (10,'(4f15.5)') (z(k,ii),k=1,48)
!!      ENDDO

      WRITE (10,*) 'Tetrahedron loaded by unit forces along the axes'

!     set the conditions w = 0 in plane (x-y) (dof 1,25,37)
!                        u = 0          (y-z) (dof 5,17,41)
!                        v = 0          (z-x) (dof 9,21,33)
!     dof at nodes (see ifix)
!     mode   u     v   w
!     1      1     5   9
!     2      13   17  21
!     3      25   29  33
!     4      37   41  45

      DO k = 1,9
         j = ifix(k)
         DO i = 1,48
            stiff(i,j) = 0.d0
            stiff(j,i) = 0.d0
         ENDDO
         stiff(j,j) = 1.d0
      ENDDO
      CALL FElib_Tetra_Decomp (48,48,stiff,scales,ips,det)
      WRITE (10,*) ' det = ',det
      f = 0d0
      f(13) = -1.d0;  f(29) = -1.d0;  f(45) = -1.d0    
      CALL FElib_Tetra_Solve (48,48,stiff,f,x,ips)
      WRITE (10,*) ' Vector x'
      WRITE (10,'(4e15.6)') (x(i),i=1,48)

!     next integration scheme 

      IF (npi.EQ.11) THEN
         npi = 14;  GOTO 10
      ELSEIF (npi.EQ.14) THEN
         npi = 15;  GOTO 10
      ELSEIF (npi.EQ.15) THEN
         npi = 24;  GOTO 10
      ENDIF

      CLOSE (UNIT=10)      
      END

      SUBROUTINE FElib_Elem103_Stiff (coor,imate,rmate,ivers,npi,stiff)

!     +------------------------------------------------------------------------+
!     | Create the stiffness matrix of an elastic tetrahedron with Hermite's   |
!     | interpolation, 4 nodes and 12 degrees of freedom per node, for a total |
!     | of 48 dof.                                                             |
!     +------------------------------------------------------------------------+
!     | coor   : nodal coordinates.                                            |
!     | imate  : integer material data.                                        |
!     |          (imate(1) = 1 for an elastic material)                        |
!     | rmate  : real material data.                                           |
!     | ivers  : version of the shape functions (may be 1 or 2).               |
!     | npi    : nb. of integration points (may be 5,11,14).                   |
!     | stiff  : upper half stiffness matrix.                                  |
!     +------------------------------------------------------------------------+

      IMPLICIT REAL*8 (a-h, o-z), INTEGER*4 (i-n)

!     Parameters:

      DIMENSION coor(3,4),imate(*),rmate(*),stiff(48,48)

!     Local variables:

      REAL*8, DIMENSION(:),     ALLOCATABLE :: weight
      REAL*8, DIMENSION(:,:),   ALLOCATABLE :: point
      REAL*8, DIMENSION(:,:,:), ALLOCATABLE :: phi

      DIMENSION d(6,6),b(6,48)

!     ==========================================================================

!     check input data

      IF (ivers.NE.1.AND.ivers.NE.2) ivers = 1
      IF (npi.NE. 5.AND.npi.NE.10.AND.npi.NE.11.AND.npi.NE.14.AND.              &
          npi.NE.15.AND.npi.NE.24.AND.npi.NE.31) npi = 5

!     allocate arrays

      ALLOCATE (point(3,npi),phi(4,16,npi),weight(npi))

!     compute the shape functions 'phi' and weights 'weight' of the tetrahedron

      CALL FElib_Tetra_Shape (coor,npi,point,ivers,phi,weight)

!     create the strain to stress matrix 'd' : sigma = d * epsilon
!     material is assumed homogeneous within the element;
!     it may, however, be isotropic or not, depending on the data 'imate' & 'rmate'.

      CALL FElib_Mate_Elastic_3D (imate,rmate,d)

!     initialize stiff

      stiff = 0.d0

!     Loop on the integration points 

      DO k = 1,npi

!        Displacements to strain matrix b(6,48); variables are numbered as follows:
!        1-12  : u1,dxu1,dyu1,dzu1, v1,dxv1,dyv1,dzv1, w1,dxw1,dyw1,dzw1 node 1
!        13-24 : u2,dxu2,dyu2,dzu2, v2,dxv2,dyv2,dzv2, w2,dxw2,dyw2,dzw2 node 2
!        25-36 : ... node 3
!        37-48 : ... node 4

         b = 0.d0

!        epsi_x = du/dx : b(1,*)

         i1 = 0;  j0 = 0
         DO i = 1,4                       ! loop on nodes
            DO j = 1,4                    ! loop on dof of the node
               j0 = j0+1                  ! index in phi
               j1 = i1+12*(i-1)+j         ! index in b
               b(1,j1) = phi(2,j0,k)
            ENDDO
         ENDDO

!        epsi_y = dv/dy : b(2,*)

         i1 = 4;  j0 = 0
         DO i = 1,4
            DO j = 1,4
               j0 = j0+1;  j1 = i1+12*(i-1)+j
               b(2,j1) = phi(3,j0,k)
            ENDDO
         ENDDO

!        epsi_z = dw/dz : b(3,*)

         i1 = 8;  j0 = 0
         DO i = 1,4
            DO j = 1,4
               j0 = j0+1;  j1 = i1+12*(i-1)+j
               b(3,j1) = phi(4,j0,k)
            ENDDO
         ENDDO

!        gamma_xy = du/dy + dv/dx : b(4,*)

         i1 = 0;  i2 = 4;  j0 = 0
         DO i = 1,4
            DO j = 1,4
               j0 = j0+1;  j1 = i1+12*(i-1)+j;  j2 = i2+12*(i-1)+j
               b(4,j1) = phi(3,j0,k)
               b(4,j2) = phi(2,j0,k)
            ENDDO
         ENDDO

!        gamma_yz = dv/dz + dw/dy : b(5,*)

         i1 = 4;  i2 = 8;  j0 = 0
         DO i = 1,4
            DO j = 1,4
               j0 = j0+1;  j1 = i1+12*(i-1)+j;  j2 = i2+12*(i-1)+j;  
               b(5,j1) = phi(4,j0,k)
               b(5,j2) = phi(3,j0,k)
            ENDDO
         ENDDO

!        gamma_zx = dw/dx + du/dz : b(6,*)

         i1 = 8;  i2 = 0;  j0 = 0
         DO i = 1,4
            DO j = 1,4
               j0 = j0+1;  j1 = i1+12*(i-1)+j;  j2 = i2+12*(i-1)+j
               b(6,j1) = phi(2,j0,k)
               b(6,j2) = phi(4,j0,k)
            ENDDO
         ENDDO

!        contribution to the stiffness matrix: stiff = stiff + trans(b)*d*b

         CALL FElib_Matrix_Symm_BtAB_Add (48,6,stiff,d,b,weight(k))
      ENDDO

!     return arrays

      DEALLOCATE (point,phi,weight)
      END

      SUBROUTINE FElib_Mate_Elastic_3D (imate,rmate,d)

!     +-----------------------------------------------------------------------+
!     | Compute the strain-stress matrix of a 3D elastic material.            |
!     +-----------------------------------------------------------------------+
!     | imate(1) = 1 for elastic material.                                    |
!     | imate(2) = 0 for isotropic homogeneous material.                      |
!     | if (imate(2) = 0)                                                     |
!     |    rmate(1) = Young's modulus.                                        |
!     |    rmate(2) = Poisson's ratio.                                        |
!     | else                                                                  |
!     |    to be defined                                                      |
!     +-----------------------------------------------------------------------+

      IMPLICIT INTEGER*4 (i-n), REAL*8 (a-h,o-z)

!     Parameters:

      DIMENSION imate(*),rmate(*),d(6,6)

!     =========================================================================

      d = 0.d0
      yg = rmate(1);  ps = rmate(2)
      d(1,1) = 1.d0;  d(2,2) = 1.d0;  d(3,3) = 1.d0
      temp = ps/(1.d0-ps)
      d(1,2) = temp;  d(1,3) = temp;  d(2,3) = temp
      temp = (1.d0-2.d0*ps)/(2.d0*(1.d0-ps))
      d(4,4) = temp;  d(5,5) = temp;  d(6,6) = temp
      temp = yg*(1.d0-ps)/((1.d0+ps)*(1.d0-2.d0*ps))
      WRITE (10,*) 'Matrix D'                    ! debug
      DO i = 1,6
         DO j = i,6
            d(i,j) = temp*d(i,j)
            d(j,i) = d(i,j)
         ENDDO
         WRITE (10,'(6d15.8)') (d(i,j),j=1,6)   ! debug
      ENDDO
      WRITE (10,'(1x)')
      END

      SUBROUTINE FElib_Matrix_Symm_BtAB_Add (n1,n2,a,b,c,w)

!     +-----------------------------------------------------------------------+
!     | Add the product trans(c)*b*c*w to the upper half matrix a.            |
!     +-----------------------------------------------------------------------+
!     | n1,n2             : matrices sizes.                                   |
!     | a(n1,n1)          : on return contains the updated matrix.            |
!     | b(n2,n2),c(n2,n1) : input matrices (b is symmetrical;                 |
!     |                                     c contains often many zeros).     |
!     | w                 : weight of the contribution.                       |
!     +-----------------------------------------------------------------------+

      IMPLICIT INTEGER*4 (i-n), REAL*8 (a-h,o-z)

!     Parameters:

      DIMENSION a(n1,n1),b(n2,n2),c(n2,n1)

!     =========================================================================

      DO i = 1,n1
          DO j = i,n1
              sum = 0.d0
              DO k = 1,n2
                  s = 0.d0
                  DO l = 1,n2
                      IF (c(l,j).NE.0.d0) s = s+b(k,l)*c(l,j)
                  ENDDO
                  IF (c(k,i).NE.0.d0) sum = sum+c(k,i)*s
              ENDDO
              a(i,j) = a(i,j)+sum*w
          ENDDO
      ENDDO
      END

