c==============================================================================
c=============  LIBRERIA PARA CALCULO MATRICIAL DE ESTRUCTURAS  ===============
c==============================================================================

c______________________________________Libreria Libcmepcger____________________
c
c AUTORES: F. Navarrina, I. Colominas, H. Gomez, M. Casteleiro
c                                                                 _
c        Dep. de Metodos Matematicos y de Rep., ETSICCP de La Coruna, UDC.
c
c FECHA ULTIMA REVISION: 20070410
c
c Copyright: Esta libreria es propiedad de su autor.
c            No puede utilizarse con fines comerciales sin conocimiento
c            y autorizacion de su autor.
c            Puede utilizarse con fines educativos sin ninguna restriccion.
c
c------------------------------------------------------------------------------
c===============================================================================
c================================== PRIMER NIVEL ===============================
c===============================================================================

c____________________________________________________________________CAL$Ini_Par
c     Inicializacion de parametros del programa

      Subroutine CAL$Ini_Par(ntype,ndime,npoin,ndofn,nnode,nstre,
     .           nprop,nevab,ntotv)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)
   
c.....Parametros que dependen del tipo de calculo
      if (ntype.eq.2) then        ! ESTRUCTURADAS ARTICULADAS 2D
        ndime=2                   ! 2 Coordenadas por punto        (x,y)
        ndofn=2                   ! 2 Grados de Libertad por Nodo  (u,v)
        nnode=2                   ! 2 Nodos por Elemento
        nstre=1                   ! 1 Esfuerzo                     (N)
                                  ! 1 Deformacion                  (dL)
        nprop=1                   ! 1 Propiedad por Elemento
                                  !      {
                                  !       1-> Rigidez a axil       (EA)
                                  !      }
      else if (ntype.eq.3) then   ! ESTRUCTURADAS ARTICULADAS 3D
        ndime=3                   ! 3 Coordenadas por punto        (x,y,z)
        ndofn=3                   ! 3 Grados de Libertad por Nodo  (u,v,w)
        nnode=2                   ! 2 Nodos por Elemento
        nstre=1                   ! 1 Esfuerzo                     (N)
                                  ! 1 Deformacion                  (dL)
        nprop=1                   ! 1 Propiedad por Elemento
                                  !      {
                                  !       1-> Rigidez a axil       (EA)
                                  !      }
      else if (ntype.eq.4) then   ! ESTRUCTURADAS RETICULADAS 2D
        ndime=2                   ! 2 Coordenadas por punto        (x,y)
        ndofn=3                   ! 1 Grado de Libertad por Nodo   (u,v,gz)
        nnode=2                   ! 2 Nodos por Elemento
        nstre=3                   ! 3 Esfuerzos                    (N,M1,M2)
                                  ! 3 Deformaciones                (dl,g1,g2)
        nprop=2                   ! 2 Propiedades por Elemento
                                  !      {
                                  !       1-> Rigidez a axil       (EA)
                                  !       2-> Rigidez a flexion    (EI)
                                  !      }
      else
        call SRV$Error('CAL$Ini_Par: Opcion no implementada (NTYPE)')
      endif

c.....Parametros de almacenamiento generales
      nevab=nnode*ndofn      ! Grados de libertad de elemento
      ntotv=npoin*ndofn      ! Grados de libertad del sistema

      return
      end

C_____________________________________________________________________CAL$Ini_AD
c     Inicializacion del almacenamiento de la diagonal principal (ensamblada)

      Subroutine CAL$Ini_AD(nelem,ntotv,nevab,lpamc,nrigi)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension lpamc(ntotv)

      do itotv=1,ntotv           ! Diagonal principal ensamblada
        lpamc(itotv)=itotv
      enddo

      nrigi=ntotv                ! Posiciones de memoria en total

      return
      end

c_________________________________________________________________CAL$Proc_Resdl
c     Procesamiento del residuo del elemento IELEM
c     K_NIV = 0 -> Calculo de matrices B,D
c           = 1 -> Calculo de matrices B,D, vector r=-Bt.(D.(Bt.u))
c           = 2 -> Calculo de matrices B,D  vector r=-Bt.(D.(Bt.u)) y ensamblaje

      Subroutine CAL$Proc_Resdl(k_niv,ntype,ielem,nelem,nnode,
     .                          npoin,ndime,ndofn,nstre,nmats,
     .                          nprop,nevab,ntotv,lnods,
     .                          coord,elcod,matno,props,
     .                          eldis,eldef,elstr,eload,elres,
     .                          dmatx,bmatx,xdisp,resdl)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension lnods(nnode,nelem),matno(nelem),props(nprop,nmats)

c.....Seleccion de coordenadas nodales del elemento
      call SRV$Sel_Subvector(nnode,lnods(1,ielem),ndime,npoin,
     .                       coord,elcod)

c.....Selecion del tipo de material
      imats=matno(ielem)

c.....Generacion del residuo elemental
      call CAL$Gen_Resdl(ntype,nnode,ndime,ndofn,nstre,
     .                   nprop,nevab,elcod,props(1,imats),
     .                   dmatx,bmatx)

      if (k_niv.eq.0) return

c.....Seleccion de desplazamientos del elemento
      call SRV$Sel_Subvector(nnode,lnods(1,ielem),ndofn,npoin,
     .                       xdisp,eldis)

c.....Integracion del residuo elemental
      dvolu=1.d+00
      call CAL$IN_Resdl(nstre,nevab,1,dvolu,
     .                  dmatx,bmatx,
     .                  eldis,eldef,elstr,eload,elres)

      if (k_niv.eq.1) return

c.....Ensamblaje del residuo de elemento en el general
      call CAL$Ensamb_Vector(nnode,ndofn,npoin,
     .                       lnods(1,ielem),elres,resdl)

      return
      end

c__________________________________________________________________CAL$Proc_RigD
c     Procesamiento de la diagonal de la matriz de rigidez del elemento IELEM
c     K_NIV = 0 -> Calculo de matrices B,D
c           = 1 -> Calculo de matrices B,D, Kd=diag(Bt.D.B)
c           = 2 -> Calculo de matrices B,D  Kd=diag(Bt.D.B) y ensamblaje de Kd

      Subroutine CAL$Proc_RigD(k_niv,ntype,ielem,nelem,nnode,
     .                         npoin,ndime,ndofn,nstre,nmats,
     .                         nprop,nevab,ntotv,lnods,
     .                         coord,elcod,matno,props,
     .                         eldis,eldef,elstr,estif,
     .                         dmatx,bmatx,rgmat)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension lnods(nnode,nelem),matno(nelem),props(nprop,nmats)

c.....Seleccion de coordenadas nodales del elemento
      call SRV$Sel_Subvector(nnode,lnods(1,ielem),ndime,npoin,
     .                       coord,elcod)

c.....Selecion del tipo de material
      imats=matno(ielem)

c.....Generacion de diagonal de matriz de rigidez elemental
      call CAL$Gen_Resdl(ntype,nnode,ndime,ndofn,nstre,
     .                   nprop,nevab,elcod,props(1,imats),
     .                   dmatx,bmatx)

      if (k_niv.eq.0) return

c.....Integracion de diagonal de matriz de rigidez elemental
      dvolu=1.d+00
      call CAL$IN_RigD(nstre,nevab,1,dvolu,
     .                 dmatx,bmatx,
     .                 eldis,eldef,elstr,
     .                 estif)

      if (k_niv.eq.1) return

c.....Ensamblaje de la diagonal de la matriz de rigidez elemental
      call CAL$Ensamb_Vector(nnode,ndofn,npoin,
     .                       lnods(1,ielem),estif,rgmat)

      return
      end

c__________________________________________________________________CAL$Proc_FPun
c     Procesamiento de la fuente puntual ILOAD de valor PFORC(ndofn)
c     aplicada en el nodo IPOIN, teniendo en cuenta los coeficientes de
c     seguridad FLOAD(ncase,iload)

      Subroutine CAL$Proc_FPun(ipoin,iload,ndofn,npoin,ntotv,
     .           nload,ncase,pforc,fload,lpntn,gload)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension pforc(ndofn),fload(ncase,nload),gload(ntotv,ncase)

      cero=0.d+00

c.....ensamblaje
      do icase=1,ncase
        if (fload(icase,iload).ne.cero) then
          call CAL$Ensamb_F(    1,ndofn,npoin,ipoin,pforc,
     .                      fload(icase,iload),lpntn,gload(1,icase))
        endif
      enddo

      return
      end

c__________________________________________________________________CAL$Proc_Tens
c     Procesamiento de intensidades en el elemento IELEM

      Subroutine CAL$Proc_Tens(ielem,nelem,ncase,nnode,ndofn,npoin,
     .                         nstre,nevab,
     .                         lnods,lpntn,xdisp,eldis,eldef,
     .                         dmatx,bmatx,strsg)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension lnods(nnode,nelem),xdisp(ndofn,npoin,ncase),
     .          strsg(nstre,ncase)

      do icase=1,ncase

c.......selecciona desplazamientos del elemento
        call SRV$Sel_Subvector_2(nnode,lnods(1,ielem),lpntn,ndofn,npoin,
     .                         xdisp(1,1,icase),eldis)

c.......calculo de tensiones
        call CAL$Cal_Tens(nstre,nevab,
     .                    dmatx,bmatx,eldis,eldef,strsg(1,icase))

      enddo

      return
      end


c===============================================================================
c=============================== SEGUNDO NIVEL =================================
c===============================================================================


c__________________________________________________________________CAL$Gen_Resdl
c     Generacion del residuo del elemento
   
      Subroutine CAL$Gen_Resdl(ntype,nnode,ndime,ndofn,nstre,
     .                         nprop,nevab,elcod,props,
     .                         dmatx,bmatx)


      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension props(nprop)

c.....calculo de la matriz D
      call CAL$Cal_D(ntype,
     .               nprop,props,nnode,ndime,elcod,
     .               nstre,dmatx)

c.....generacion de la matriz B
      call CAL$Cal_B(ntype,
     .               nnode,ndime,elcod,
     .               nevab,nstre,bmatx)

      return
      end

c===============================================================================
c=========================== INTEGRACION NUMERICA ==============================
c===============================================================================


c___________________________________________________________________CAL$IN_Resdl
c                                              _   t       _
c     Integracion numerica del residuo         r=-B .(D.(B.u))
c                                                 ~   ~  ~

      Subroutine CAL$IN_Resdl(nstre,nevab,ngaue,dvolu,
     .                        dmatx,bmatx,
     .                        eldis,eldef,elstr,eload,elres)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension dvolu(ngaue),bmatx(nstre,nevab,ngaue),
     .          eload(nevab),elres(nevab)

      cero=0.d+00

      call SRV$Ini_Vector(nevab,elres)

      do igaue=1,ngaue
        call CAL$Cal_Tens(nstre,nevab,
     .                    dmatx,bmatx(1,1,igaue),eldis,eldef,elstr)
        call SRV$Mult_Mt_v(nevab,nstre,bmatx(1,1,igaue),elstr,eload)
        do ievab=1,nevab
          elres(ievab)=elres(ievab)-dvolu(igaue)*eload(ievab)
        enddo
      enddo

      return
      end

c____________________________________________________________________CAL$IN_RigD
c                                                  t  
c     Integracion numerica de la diagonal de K =  B .D.B
c                                                 ~  ~ ~

      Subroutine CAL$IN_RigD(nstre,nevab,ngaue,dvolu,
     .                       dmatx,bmatx,
     .                       eldis,eldef,elstr,
     .                       estif)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension dvolu(ngaue),bmatx(nstre,nevab,ngaue),
     .          eldis(nevab),estif(nevab)

      cero=0.d+00
      uno=1.d+00

      call SRV$Ini_Vector(nevab,eldis)
      do ievab=1,nevab 
        eldis(ievab)=uno

        estif(ievab)=cero
        do igaue=1,ngaue
          call CAL$Cal_Tens(nstre,nevab,
     .                      dmatx,bmatx(1,1,igaue),eldis,eldef,elstr)
          call SRV$Mult_Mt_v(   1,nstre,eldef,elstr,estid)
          estif(ievab)=estif(ievab)+dvolu(igaue)*estid
        enddo

        eldis(ievab)=cero
      enddo

      return
      end

c===============================================================================
c=========================== FORMULACION MATRICIAL =============================
c===============================================================================

c______________________________________________________________________CAL$Cal_D
c     Calculo de la matriz D
c                          ~

      Subroutine CAL$Cal_D(ntype,
     .                     nprop,props,nnode,ndime,elcod,
     .                     nstre,dmatx)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension props(nprop),elcod(ndime,nnode),dmatx(nstre,nstre)

      cero=0.d+00

      if (ntype.eq.2) then
        ea=props(1)
        dx=elcod(1,2)-elcod(1,1)
        dy=elcod(2,2)-elcod(2,1)
        rl=dsqrt(dx*dx+dy*dy)
        dmatx(1,1)=ea/rl
      else if (ntype.eq.3) then
        ea=props(1)
        dx=elcod(1,2)-elcod(1,1)
        dy=elcod(2,2)-elcod(2,1)
        dz=elcod(3,2)-elcod(3,1)
        rl=dsqrt(dx*dx+dy*dy+dz*dz)
        dmatx(1,1)=ea/rl
      else if (ntype.eq.4) then
        ea=props(1)
        ei=props(2)
        dx=elcod(1,2)-elcod(1,1)
        dy=elcod(2,2)-elcod(2,1)
        rl=dsqrt(dx*dx+dy*dy)
        dmatx(1,1)=ea/rl
        dmatx(2,1)=cero
        dmatx(3,1)=cero
        dmatx(1,2)=cero
        dmatx(2,2)=4.d+00*ei/rl
        dmatx(3,2)=2.d+00*ei/rl
        dmatx(1,3)=cero
        dmatx(2,3)=dmatx(3,2)
        dmatx(3,3)=dmatx(2,2)
      else
        call SRV$Error('CAL$CAL$Cal_D: Opcion no implementada (NTYPE)')
      endif

      return
      end

c______________________________________________________________________CAL$Cal_B
c     Calculo de la matriz B
c                          ~

      Subroutine CAL$Cal_B(ntype,
     .                     nnode,ndime,elcod,
     .                     nevab,nstre,bmatx)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension elcod(ndime,nnode),bmatx(nstre,nevab)

      cero=0.d+00
      uno= 1.d+00

      if (ntype.eq.2) then
        dx=elcod(1,2)-elcod(1,1)
        dy=elcod(2,2)-elcod(2,1)
        rl=dsqrt(dx*dx+dy*dy)
        bmatx(1,1)=-dx/rl
        bmatx(1,2)=-dy/rl
        bmatx(1,3)=-bmatx(1,1)
        bmatx(1,4)=-bmatx(1,2)
      else if (ntype.eq.3) then
        dx=elcod(1,2)-elcod(1,1)
        dy=elcod(2,2)-elcod(2,1)
        dz=elcod(3,2)-elcod(3,1)
        rl=dsqrt(dx*dx+dy*dy+dz*dz)
        bmatx(1,1)=-dx/rl
        bmatx(1,2)=-dy/rl
        bmatx(1,3)=-dz/rl
        bmatx(1,4)=-bmatx(1,1)
        bmatx(1,5)=-bmatx(1,2)
        bmatx(1,6)=-bmatx(1,3)
      else if (ntype.eq.4) then
        dx=elcod(1,2)-elcod(1,1)
        dy=elcod(2,2)-elcod(2,1)
        rl=dsqrt(dx*dx+dy*dy)
        bmatx(1,1)=-dx/rl
        bmatx(2,1)=-dy/rl/rl
        bmatx(3,1)= bmatx(2,1)
        bmatx(1,2)=-dy/rl
        bmatx(2,2)= dx/rl/rl
        bmatx(3,2)= bmatx(2,2)
        bmatx(1,3)= cero
        bmatx(2,3)= uno
        bmatx(3,3)= cero
        bmatx(1,4)=-bmatx(1,1)
        bmatx(2,4)=-bmatx(2,1)
        bmatx(3,4)=-bmatx(3,1)
        bmatx(1,5)=-bmatx(1,2)
        bmatx(2,5)=-bmatx(2,2)
        bmatx(3,5)=-bmatx(3,2)
        bmatx(1,6)= cero
        bmatx(2,6)= cero
        bmatx(3,6)= uno
      else
        call SRV$Error('CAL$CAL$Cal_B: Opcion no implementada (NTYPE)')
      endif

      return
      end


c___________________________________________________________________CAL$Cal_Tens
c                                 _        _
c     Calculo de las intensidades s = D (B u)
c                                     ~  ~

      Subroutine CAL$Cal_Tens(nstre,nevab,
     .                        dmatx,bmatx,eldis,eldef,elstr)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension dmatx(nstre,nstre),bmatx(nstre,nevab),
     .          eldis(nevab),eldef(nstre),elstr(nstre),eload(nevab)

c.....Producto: ELDEF(nstre)=BMATX(nstre,nevab)*ELDIS(nevab)
      call SRV$Mult_M_v(nstre,nevab,bmatx,eldis,eldef)

c.....Producto: ELSTR(nstre)=DMATX(nstre,nstre)*ELDEF(nstre)
      call SRV$Mult_M_v(nstre,nstre,dmatx,eldef,elstr)

      return
      end


c===============================================================================
c============================= GESTION DE MEMORIA ==============================
c===============================================================================

c___________________________________________________________________CAL$Ensamb_F
c     Ensamblaje del vector de fuerzas de elemento (ELOAD)
c     en el vector de fuerzas global (GLOAD)
c     considerando la renumeracion nodal (LPNTN)
c     y teniendo en cuenta el coeficiente de seguridad (FLOAD)

      Subroutine CAL$Ensamb_F(nnode,ndofn,npoin,
     .                        lnods,eload,fload,lpntn,gload)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)
   
      dimension lnods(nnode),eload(ndofn,nnode),lpntn(npoin),
     .          gload(ndofn,npoin)

      do inode=1,nnode
        ipoir=lpntn(lnods(inode))
        do idofn=1,ndofn
          gload(idofn,ipoir)=gload(idofn,ipoir)+
     .                       eload(idofn,inode)*fload
        enddo
      enddo

      return
      end

c______________________________________________________________CAL$Ensamb_Vector
c     Ensamblaje de un vector de elemento (EVECT) en el vector global (GVECT)

      Subroutine CAL$Ensamb_Vector(nnode,ndofn,npoin,lnods,evect,gvect)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)
   
      dimension lnods(nnode),evect(ndofn,nnode),gvect(ndofn,npoin)

      do inode=1,nnode
        ipoin=lnods(inode)
        do idofn=1,ndofn
          gvect(idofn,ipoin)=gvect(idofn,ipoin)+evect(idofn,inode)
        enddo
      enddo

      return
      end

C_________________________________________________________________CAL$Imp_Vincul
c     Formacion del vector de grados de libertad prescritos (NGFIX)
c     a partir de las condiciones de vinculacion (NOFIX)
c     considerando la renumeracion nodal (LPNTN)

      Subroutine CAL$Imp_Vincul(nvfix,ndofn,npoin,nofix,ngfix,lpntn)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension nofix(2,nvfix),ngfix(nvfix),lpntn(npoin)

      do ivfix=1,nvfix
        ipoin=nofix(1,ivfix)
        idofn=nofix(2,ivfix)
        ipoir=lpntn(ipoin)
        itotv=(ipoir-1)*ndofn+idofn
        ngfix(ivfix)=itotv
      enddo

      return
      end


c===============================================================================
c======================== Subrutinas Generales de Servicio =====================
c===============================================================================

c_________________________________________________________________SRV$Ini_Vector
c     Inicializacion de un vector a cero

      Subroutine SRV$Ini_Vector(n,v)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension v(n)

      cero=0.d+00

      do i=1,n
        v(i)=cero
      enddo

      return
      end

c_________________________________________________________________SRV$Cop_Vector
c     Copia un vector en otro

      Subroutine SRV$Cop_Vector(n,v,w)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension v(n),w(n)

      do i=1,n
        w(i)=v(i)
      enddo

      return
      end

c______________________________________________________________SRV$Sel_Subvector
c     Seleccion de un subvector mediante un redireccionamiento

      Subroutine SRV$Sel_Subvector(nn,ln,nd,np,co,el)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension ln(nn),co(nd,np),el(nd,nn)

      do in=1,nn
        ip=ln(in)
        do id=1,nd
          el(id,in)=co(id,ip)
        enddo
      enddo

      return
      end

c____________________________________________________________SRV$Sel_Subvector_2
c     Seleccion de un subvector mediante dos redireccionamientos consecutivos

      Subroutine SRV$Sel_Subvector_2(nn,ln,ln2,nd,np,co,el)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension ln(nn),ln2(np),co(nd,np),el(nd,nn)

      do in=1,nn
        ip=ln2(ln(in))
        do id=1,nd
          el(id,in)=co(id,ip)
        enddo
      enddo

      return
      end

c___________________________________________________________________SRV$Mult_M_v
c                        _
c     Producto Matricial c         = A         . b
c                                    ~ (n x l)

      Subroutine SRV$Mult_M_v(n,l,a,b,c)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension a(n,l),b(l),c(n)

      cero=0.d+00

      do i=1,n
        c(i)=cero
        do k=1,l
          c(i)=c(i)+a(i,k)*b(k)
        enddo  
      enddo

      return
      end

c__________________________________________________________________SRV$Mult_Mt_v
c                        _             T         _
c     Producto Matricial c         = A         . b
c                                    ~ (l x n)

      Subroutine SRV$Mult_Mt_v(n,l,a,b,c)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      dimension a(l,n),b(l),c(n)

      cero=0.d+00

      do i=1,n
        c(i)=cero
        do k=1,l
          c(i)=c(i)+a(k,i)*b(k)
        enddo
      enddo

      return
      end

c______________________________________________________________________SRV$Error
c     Impresion de mensaje de error y detencion del programa

      Subroutine SRV$Error(mensaje)

      implicit integer*4 (i-n)
d     implicit real*8    (a-h,o-z)

      character*(*) mensaje

      write(6,100) mensaje
 100  format(/' ERROR > ',a/)

      stop
      end
