c==============================================================================
c============    SOLUCION DE SISTEMAS DE ECUACIONES LINEALES    ===============
c==============================================================================

c______________________________________SUBRUTINA Solver_LDLt_CP________________
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: 20070221
c
c Copyright: Esta subrutina 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 OBJETIVO:
c
c    Solucion general del sistema de ecuaciones :
c
c                                         -   -   -
c                                     A . x = b + r
c                                     ~            v
c    con las condiciones adicionales
c                                     _     _
c                                     x   = p       
c                                      v     v
c    siendo:
c           A = matriz de coeficientes  (matriz de rigidez)         [dato]
c           x = solucion                (vector de desplazamientos) [incognita]
c           b = terminos independientes (vector de fuerzas nodales) [dato]
c           r = reacciones en gdl coaccionados                      [incognita]
c            v
c           p = valores prescritos de los gdl coaccionados          [dato]
c            v
c------------------------------------------------------------------------------
c 
c DESCRIPCION GENERAL:
c
c       La matriz A es simetrica y puede ser singular.
c
c       Su parte triangular inferior (incluida la diagonal) se almacena por
c    filas en Column Profile.
c
c       Se asume que al imponerle al sistema las condiciones de vinculacion
c    (gdl coaccionados) se obtiene una matriz definida positiva (no singular).
c       
c       El sistema resultante A'. x = b' se resuelve mediante el metodo 
c    avanzado de Cholesky descomponiendo la matriz en un producto LDLt 
c    y resolviendo el sistema resultante mediante las adecuadas 
c    sustituciones hacia atras y hacia adelante.
c
c             A'. x = b'        ]         [    L . z = b'
c                               ]  -----> [    D . y = z
c             L . D . Lt= A'    ]         [    Lt. x = y
c
c       Posteriormente se calculan las reacciones en los gdl coaccionados.
c
c       Pueden resolverse uno o varios sistemas de ecuaciones simultaneamente.
c
c       Posteriormente pueden resolverse nuevos sistemas de ecuaciones
c    aprovechando la descomposicion anterior, siempre y cuando los gdl
c    coaccionados sean los mismos.
c
c------------------------------------------------------------------------------
c
c VARIABLES:
c
c     kproc = [Entrada]: indicador del tipo de proceso a realizar.
c
c         0 -> -Descomposicion de la matriz A' en el producto LDLt
c              -Imposicion de condiciones de vinculacion sobre el vector b->b'
c              -Solucion del sistema de ecuaciones LDLt. x = b'
c              -Calculo de reacciones
c
c         1 -> -Imposicion de condiciones de vinculacion sobre el vector b->b'
c              -Solucion del sistema de ecuaciones LDLt. x = b'
c              -Calculo de reacciones
c
c         NOTA: La opcion 1 solo podra emplearse tras haber seleccionado
c               anteriormente la opcion 0.
c
c     ntotv = [Entrada]: numero de ecuaciones.
c     lpamc = [Entrada]: vector de direccionamiento del column profile.
c     rgmat = [Entrada]: triangulo inferior (incluida la diagonal)
c                        de la matriz de coeficientes 
c                        almacenado por filas en Column Profile.
c             [Salida ]: matriz L' de la descomposicion de Cholesky
c                        de la matriz de coeficientes
c                        (con las condiciones de vinculacion dadas),
c                        almacenada por filas en Column Profile.
c                        Su diagonal principal corresponde a la matriz D.
c                        Los coeficientes correspondientes a gdl coaccionados
c                        no se modifican.
c     nvfix = [Entrada]: numero de gdl coaccionados.
c     ngfix = [Entrada]: lista de gdl coaccionados.
c     presc = [Entrada]: lista de valores prescritos en gdl coaccionados.
c     ncase = [Entrada]: numero de sistemas de ecuaciones a resolver
c                        simultaneamente.
c     gload = [Entrada]: matriz de terminos independientes.
c             [Salida ]: matriz de soluciones de los sistemas.
c     treac = [Salida ]: matriz de valores de las reacciones.
c
c
c------------------------------------------------------------------------------

      subroutine SLE$Solver_LDLt_CP(kproc,
     .                              ntotv,lpamc,rgmat,
     .                              nvfix,ngfix,presc,
     .                              ncase,gload,treac)
d     implicit real*8 (a-h,o-z)
      dimension gload(ntotv,ncase),treac(nvfix,ncase)


c.....Cambio de signo a punteros de gdl restringidos
      if (nvfix.gt.0) call SLE$MarcarCoacciones(ntotv,lpamc,nvfix,ngfix)

c.....Descomposicion de la matriz K' en el producto LDLt
      if (kproc.eq.0) call SLE$Factorizar_LDLt(ntotv,lpamc,rgmat)

c.....Imposicion de coacciones sobre terminos independientes y reacciones
      if (nvfix.gt.0) then
        do icase=1,ncase
          call SLE$ImponerCoacciones(ntotv,lpamc,rgmat,
     .                               nvfix,ngfix,presc,
     .                               gload(1,icase),treac(1,icase))
        enddo
      endif

c.....Solucion de sistemas de ecuaciones LDLt. x' = b'
      do icase=1,ncase
        call SLE$Sustituir_LDLt(ntotv,lpamc,rgmat,gload(1,icase))
      enddo

c.....Calculo de reacciones en gdl coaccionados
      if (nvfix.gt.0) then
        do icase=1,ncase
          call SLE$CalcularReacciones(ntotv,lpamc,rgmat,
     .                                nvfix,ngfix,
     .                                gload(1,icase),treac(1,icase))
        enddo
      endif

c.....Restauracion del signo a punteros de gdl restringidos
      if (nvfix.gt.0) call SLE$MarcarCoacciones(ntotv,lpamc,nvfix,ngfix)

      return
      end

c______________________________________SUBRUTINA MarcarCoacciones______________
c
c     Cambio de signo de punteros de gdl restringidos
c
c     -------------------------------------------------------------------------
c     n   = [Entrada]: dimension de la matriz A
c                      = numero de ecuaciones = numero de incognitas.
c     lp  = [Entrada]: vector de punteros de los elementos de la diagonal.
c     nv  = [Entrada]: numero de gdl coaccionados.
c     ng  = [Entrada]: lista de gdl coaccionados.
c     -------------------------------------------------------------------------

      subroutine SLE$MarcarCoacciones(n,lp,nv,ng)
      dimension  lp(n),ng(nv)

c.....bucle sobre coacciones
      do iv=1,nv
        i=ng(iv)
        lp(i)=-lp(i)
      enddo

      return
      end

c______________________________________SUBRUTINA ImponerCoacciones_____________
c
c     Imposicion de las condiciones de vinculacion sobre el vector b e
c     inicializacion de reacciones.
c
c     -------------------------------------------------------------------------
c     n   = [Entrada]: dimension de la matriz A
c                      = numero de ecuaciones = numero de incognitas.
c     lp  = [Entrada]: vector de punteros de los elementos de la diagonal
c                      (puntero negativo => el gdl esta coaccionado).
c     a   = [Entrada]: triangulo inferior (incluida la diagonal)
c                      de la matriz, almacenada por filas en Column Profile.
c     nv  = [Entrada]: numero de gdl coaccionados.
c     ng  = [Entrada]: lista de gdl coaccionados.
c     pr  = [Entrada]: lista de valores prescritos en los gdl coaccionados.
c     b   = [Entrada]: vector b  de terminos independientes
c           [Salida ]: vector b' de terminos independientes
c                      (con los valores prescritos en los gdl coaccionados).
c     tr  = [Salida ]: vector de reacciones inicializado
c     -------------------------------------------------------------------------

      subroutine SLE$ImponerCoacciones(n,lp,a,nv,ng,pr,b,tr)
d     implicit real*8 (a-h,o-z)
      dimension lp(n),a(1),ng(nv),pr(nv),b(n),tr(nv)

c.....bucle sobre g.d.l. coaccionados
      do iv=1,nv
        j=ng(iv)

c.......adaptacion de terminos independientes
        if (j.gt.1) then
          lpjj=iabs(lp(j))
          lpj0=lpjj-j
          lsbj=lpjj-(iabs(lp(j-1))+1)
          do i=j-lsbj,j-1                                   ! zona superior
            if (lp(i).gt.0) then
              lpji=lpj0+i
              b(i)=b(i)-a(lpji)*pr(iv)
            endif
          enddo
        endif

        do i=j+1,n                                          ! zona inferior
          if (lp(i).gt.0) then
            lpii=iabs(lp(i))
            lpi0=lpii-i
            lsbi=lpii-(iabs(lp(i-1))+1)
            if (i-lsbi.le.j) then
              lpij=lpi0+j
              b(i)=b(i)-a(lpij)*pr(iv)
            endif
          endif
        enddo

c.......inicializacion de la reaccion e imposicion de la coaccion
        tr(iv)=-b(j)
        b(j)=pr(iv)

      enddo

      return
      end

c______________________________________SUBRUTINA Factorizar_LDLt_______________
c
c     Factorizacion de Cholesky (A = L D Lt) de la Matriz A - Simetrica 
c                                                           - Definida Positiva
c                                                           - en Column Profile
c     -------------------------------------------------------------------------
c     n   = [Entrada]: dimension de la matriz A.
c     lp  = [Entrada]: vector de punteros de los elementos de la diagonal
c                      (puntero negativo => el gdl esta coaccionado).
c     a   = [Entrada]: triangulo inferior (incluida la diagonal)
c                      de la matriz, almacenada por filas en Column Profile.
c           [Salida ]: matriz L de la descomposicion de Cholesky
c                      de la matriz, almacenada por filas en Column Profile.
c                      Su diagonal principal corresponde a la matriz D.
c     -------------------------------------------------------------------------

      subroutine SLE$Factorizar_LDLt(n,lp,a)
d     implicit real*8 (a-h,o-z)
      dimension lp(n),a(1)
      cero=.0d+00

      k=1
      if (lp(k).gt.0) then
        lpkk=iabs(lp(k))
        if (a(lpkk).eq.cero) call SLE$FError(' pivot nulo en fila:',k)
      endif

c.....bucle sobre filas
      do k=2,n
        if (lp(k).gt.0) then
          lpkk=iabs(lp(k))
          lpk0=lpkk-k
          lsbk=lpkk-(iabs(lp(k-1))+1)

c.........solucion del sistema triangular inferior
          do i=jmax0(2,k-lsbk+1),k-1
            if (lp(i).gt.0) then
              lpii=iabs(lp(i))
              lpi0=lpii-i
              lsbi=lpii-(iabs(lp(i-1))+1)
              s=cero
              do j=jmax0(i-lsbi,k-lsbk),i-1
                if (lp(j).gt.0) then
                  lpij=lpi0+j
                  lpkj=lpk0+j
                  s=s+a(lpij)*a(lpkj)
                endif
              enddo
              lpki=lpk0+i
              a(lpki)=a(lpki)-s
            endif
          enddo

c.........solucion del sistema diagonal
          do i=k-lsbk,k-1
            if (lp(i).gt.0) then
              lpii=iabs(lp(i))
              lpki=lpk0+i
              a(lpki)=a(lpki)/a(lpii)
            endif
          enddo

c.........calculo del elemento de la diagonal
          s=cero
          do j=k-lsbk,k-1
            if (lp(j).gt.0) then
              lpjj=iabs(lp(j))
              lpkj=lpk0+j
              s=s+a(lpkj)*a(lpjj)*a(lpkj)
            endif
          enddo
          a(lpkk)=a(lpkk)-s
          if (a(lpkk).eq.cero) call SLE$FError(' pivot nulo en fila:',k)

        endif
      enddo   ! fin de bucle sobre filas

      return
      end

c______________________________________SUBRUTINA Sustituir_LDLT________________
c
c     Resuelve - por sustitucion hacia delante sobre la matriz L, 
c              - por inversion de la matriz diagonal D, y
c              - por sustitucion hacia atras sobre la matriz Lt,
c     un sistema de ecuaciones lineales  (L . D . Lt) x = b'
c     siendo L una matriz triangular inferior,
c     con las matrices L y D almacenadas por filas en Column Profile
c
c     -------------------------------------------------------------------------
c     n   = [Entrada]: dimension de la matriz A
c                      = numero de ecuaciones = numero de incognitas.
c     lp  = [Entrada]: vector de punteros de los elementos de la diagonal
c                      (puntero negativo => el gdl esta coaccionado).
c     a   = [Entrada]: matriz L de la descomposicion de Cholesky
c                      de la matriz, almacenada por filas en Column Profile.
c                      Su diagonal principal corresponde a la matriz D.
c     b   = [Entrada]: vector b' de terminos independientes
c           [Salida ]: vector x de soluciones
c     -------------------------------------------------------------------------

      subroutine SLE$Sustituir_LDLt(n,lp,a,b)
d     implicit real*8 (a-h,o-z)
      dimension lp(n),a(1),b(n)
      cero=.0d+00

c.....sustitucion hacia adelante con L.
      do i=2,n
        if (lp(i).gt.0) then
          lpii=iabs(lp(i))
          lpi0=lpii-i
          lsbi=lpii-(iabs(lp(i-1))+1)
          s=cero
          do j=i-lsbi,i-1
            if (lp(j).gt.0) then
              lpij=lpi0+j
              s=s+a(lpij)*b(j)
            endif
          enddo
          b(i)=b(i)-s
        endif
      enddo

c.....solucion del sistema diagonal con D.
      do i=1,n
        if (lp(i).gt.0) then
          lpii=iabs(lp(i))
          b(i)=b(i)/a(lpii)
        endif
      enddo

c.....sustitucion hacia atras con Lt.
      do i=n,2,-1
        if (lp(i).gt.0) then
          lpii=iabs(lp(i))
          lpi0=lpii-i
          lsbi=lpii-(iabs(lp(i-1))+1)
          do j=i-lsbi,i-1
            if (lp(j).gt.0) then
              lpij=lpi0+j
              b(j)=b(j)-a(lpij)*b(i)
            endif
          enddo
        endif
      enddo

      return
      end

c______________________________________SUBRUTINA CalcularReacciones____________
c
c     Calculo de reacciones en gdl coaccionados
c
c     n   = [Entrada]: dimension de la matriz A
c                      = numero de ecuaciones = numero de incognitas.
c     lp  = [Entrada]: vector de punteros de los elementos de la diagonal
c                      (puntero negativo => el gdl esta coaccionado).
c     a   = [Entrada]: matriz L de la descomposicion de Cholesky
c                      de la matriz, almacenada por filas en Column Profile.
c                      Su diagonal principal corresponde a la matriz D.
c     nv  = [Entrada]: numero de gdl coaccionados.
c     ng  = [Entrada]: lista de gdl coaccionados.
c     b   = [Entrada]: vector x de soluciones
c     tr  = [Entrada]: vector de reacciones inicializado
c           [Salida ]: vector de reacciones
c     -------------------------------------------------------------------------

      subroutine SLE$CalcularReacciones(n,lp,a,nv,ng,b,tr)
d     implicit real*8 (a-h,o-z)
      dimension lp(n),a(1),ng(nv),b(n),tr(nv)

c.....bucle sobre gdl coaccionados
      do iv=1,nv
        i=ng(iv)

c.......calculo de la reaccion
        lpii=iabs(lp(i))
        tr(iv)=tr(iv)+a(lpii)*b(i)              ! diagonal
        if (i.gt.1) then
          lpi0=lpii-i
          lsbi=lpii-(iabs(lp(i-1))+1)
          do j=i-lsbi,i-1                       ! zona inferior
            lpij=lpi0+j
            tr(iv)=tr(iv)+a(lpij)*b(j)
          enddo
        endif

        do j=i+1,n                              ! zona superior
          lpjj=iabs(lp(j))
          lpj0=lpjj-j
          lsbj=lpjj-(iabs(lp(j-1))+1)
          if (j-lsbj.le.i) then
            lpji=lpj0+i
            tr(iv)=tr(iv)+a(lpji)*b(j)
          endif
        enddo

      enddo

      return
      end

c______________________________________SUBRUTINA FError________________________

      subroutine SLE$FError(alfa,i)
      character alfa*(*)

      write(6,'(2a,i5)') ' SLE> ',alfa,i

      stop
      end

