C
C N5-truss.f Finite Element Method 
C @Zitny 2000 
C 2D-truss  
C
      parameter (num=50,nem=50)
      common /grid/x(num),y(num),nc(2,nem),a(nem)
      common am(10000),fm(100),lf(num),f(num),ld(num),d(num)
C Read grid data
      open(1,file='N5-truss.dat')
C
C Data must be prepared in the file N5-truss.dat in the following format
C 1.line empty (or any text - heading)
C 2.line nu,ne - number of nodes, elements
C 3.line E-modulus of eleasticity
C 4.line and following x1,y1, x2,y2,.....  coordinates of nodes (nu-lines)
C 5.connectivity matrix nc and area a (ne-lines)
C                      nc1,1, nc2,1, a1,
C                      nc1,2, nc2,2, a2,
C                      .....                
C 6.number of prescribed forces  nf and nf-lines, defining node and force:
C     lf1, f1
C     lf2, f2
C ....... lfi - absolute value of node, fi-component of force (x-if lfi>0, or y if lfi<0)
C 7. number of prescribed displacements  nd, and nd-lines defining node and displacement:
C     ld1, d1
C     ld2, d2
C ....... ldi - absolute value of node, di-component of displacement (x-if ldi>0, or y if ldi<0)
C
      read(1,*)
      read(1,*)nu,ne
      read(1,*)e
      read(1,*)(x(i),y(i),i=1,nu)
      read(1,*)((nc(j,i),j=1,2),a(i),i=1,ne)
      read(1,*)nf,(lf(i),f(i),i=1,nf)
      read(1,*)nd,(ld(i),d(i),i=1,nf)
      neq=2*nu
C Assembly of stiffness matrix and R.H.S. vector
      call gassem(neq,ne,nu,nc,x,y,e,a,nf,lf,f,nd,ld,d,am,fm)
C Solution of linear equations by Gauss elimination (routine Numerical recipes)
      call gaussj(am,neq,neq,fm,1,1)
C Output Results
      open(2,file='N5-truss.out')
      write(2,1)nu,(x(i),y(i),i=1,nu)
      write(2,2)ne,((nc(j,i),j=1,2),a(i),i=1,ne)
      write(2,3)(i,fm(2*i-1),fm(2*i),i=1,nu)
1     format(i3/(2e10.3))
2     format(i3/(2i5,e10.3))
3     format(i3,2e10.3)
      end

      subroutine gassem(neq,ne,nu,nc,x,y,e,a,nf,lf,f,nd,ld,d,am,fm)
      dimension x(nu),y(nu),a(ne),nc(2,ne),am(neq,neq),fm(neq),
     /          al(4,4),xl(2),yl(2),ieq(4),lf(nf),f(nf),ld(nd),d(nd)
C AM - stiffness matrix of system
C FM - R.H.S. vector of system      
      do i=1,neq
      do j=1,neq
        am(i,j)=0
      enddo
        fm(i)=0
      enddo
C Element loop (NC(1..3, 1..NE)-connectivity matrix)
      do ie=1,ne
         do i=1,2
            iu=nc(i,ie)
            xl(i)=x(iu)
            yl(i)=y(iu)
            ieq(2*i-1)=2*iu-1
            ieq(2*i)=2*iu
         enddo
         call klocal(xl,yl,e,a(ie),al)
C ASSEMBLY
         do i=1,4
            do j=1,4
C Add local matrix (4 x 4) to the global matrix (NEQ x NEQ)
               am(ieq(i),ieq(j))=am(ieq(i),ieq(j))+al(i,j)
            enddo
         enddo
      enddo
C RHS vector of external forces
      do jf=1,nf
              if(lf(jf).gt.0)then
                            fm(2*lf(jf)-1)=f(jf)
              else
                            fm(-2*lf(jf))=f(jf)
              endif
      enddo
C Prescribed displacements
      do ju=1,nd
              if(ld(ju).gt.0)then
                            i=2*ld(ju)-1
              else
                            i=-2*ld(ju)
              endif
              am(i,i)=am(i,i)*1e10
              fm(i)=am(i,i)*d(ju)
      enddo
      end
      
      subroutine klocal(x,y,e,a,kl)
      dimension x(2),y(2),kl(4,4)
      real l,kl
C Local STIFFNESS MATRIX AL(4 x 4)
      l=sqrt((x(1)-x(2))**2+(y(1)-y(2))**2)
      ss=(y(2)-y(1))/l
      cs=(x(2)-x(1))/l
      rk=e*a/l
         kl(1,1)=rk*cs**2
         kl(1,2)=rk*cs*ss
         kl(1,3)=rk*(-(cs)**2)
         kl(1,4)=rk*(-(cs)*ss)
         kl(2,1)=rk*cs*ss
         kl(2,2)=rk*ss**2
         kl(2,3)=rk*(-(cs)*ss)
         kl(2,4)=rk*(-(ss)**2)
         kl(3,1)=rk*((-cs)**2)
         kl(3,2)=rk*((-cs)*ss)
         kl(3,3)=rk*cs**2
         kl(3,4)=rk*cs*ss
         kl(4,1)=rk*((-cs)*ss)
         kl(4,2)=rk*((-ss)**2)
         kl(4,3)=rk*cs*ss
         kl(4,4)=rk*ss**2
      end
      
      subroutine gaussj(a,n,np,b,m,mp)
C
C Numerical Recipes p. 28
C
      parameter (nmax=100)
      dimension a(np,np),b(np,mp),ipiv(nmax),indxr(nmax),indxc(nmax)
      do i=1,n
         ipiv(i)=0
      enddo
      do i=1,n
         big=0
         do j=1,n
             if(ipiv(j).ne.1)then
                   do k=1,n
                         if(ipiv(k).eq.0)then
                               if(abs(a(j,k)).ge.big)then
                                    big=abs(a(j,k))
                                    irow=j
				    icol=k
                               endif
                         elseif(ipiv(k).gt.1)then
                               pause 'Singular matrix'
                         endif
                    enddo
               endif
          enddo
          ipiv(icol)=ipiv(icol)+1
          if(irow.ne.icol)then
              do l=1,n
                 dum=a(irow,l)
                 a(irow,l)=a(icol,l)
                 a(icol,l)=dum
              enddo
              do l=1,m
                 dum=b(irow,l)
                 b(irow,l)=b(icol,l)
                 b(icol,l)=dum
              enddo
          endif
          indxr(i)=irow
          indxc(i)=icol
          if(a(icol,icol).eq.0.) pause 'Singular'
          pivinv=1/a(icol,icol)
          a(icol,icol)=1
          do l=1,n
             a(icol,l)=a(icol,l)*pivinv
          enddo
          do l=1,m
             b(icol,l)=b(icol,l)*pivinv
          enddo
          do ll=1,n
              if(ll.ne.icol)then
                  dum=a(ll,icol)   
                  a(ll,icol)=0
                  do l=1,n
                     a(ll,l)=a(ll,l)-a(icol,l)*dum
                  enddo
                  do l=1,m
                     b(ll,l)=b(ll,l)-b(icol,l)*dum
                  enddo
               endif
           enddo
      enddo
      do l=n,1,-1
         if(indxr(l).ne.indxc(l))then
            do k=1,n
               dum=a(k,indxr(l))
               a(k,indxr(l))=a(k,indxc(l))
               a(k,indxc(l))=dum
            enddo
         endif
      enddo
      end

