lag_mult_solv.F

Go to the documentation of this file.

Copyright>        OpenRadioss
Copyright>        Copyright (C) 1986-2025 Altair Engineering Inc.
Copyright>
Copyright>        This program is free software: you can redistribute it and/or modify
Copyright>        it under the terms of the GNU Affero General Public License as published by
Copyright>        the Free Software Foundation, either version 3 of the License, or
Copyright>        (at your option) any later version.
Copyright>
Copyright>        This program is distributed in the hope that it will be useful,
Copyright>        but WITHOUT ANY WARRANTY; without even the implied warranty of
Copyright>        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
Copyright>        GNU Affero General Public License for more details.
Copyright>
Copyright>        You should have received a copy of the GNU Affero General Public License
Copyright>        along with this program.  If not, see <https://www.gnu.org/licenses/>.
Copyright>
Copyright>
Copyright>        Commercial Alternative: Altair Radioss Software
Copyright>
Copyright>        As an alternative to this open-source version, Altair also offers Altair Radioss
Copyright>        software under a commercial license.  Contact Altair to discuss further if the
Copyright>        commercial version may interest you: https://www.altair.com/radioss/.
!||====================================================================
!||    lag_mult_solv   ../engine/source/tools/lagmul/lag_mult_solv.F
!||--- called by ------------------------------------------------------
!||    lag_mult        ../engine/source/tools/lagmul/lag_mult.F
!||--- calls      -----------------------------------------------------
!||    cholfact        ../engine/source/tools/lagmul/cholfact.F
!||    lag_mult_h      ../engine/source/tools/lagmul/lag_mult_h.F
!||    prechol         ../engine/source/tools/lagmul/cholfact.F
!||====================================================================
      SUBROUTINE lag_mult_solv(
     1                    NH    ,NC    ,NCR   ,A     ,V     ,
     2                    MAS   ,IADLL ,LLL   ,JLL   ,XLL   ,
     3                    IADH  ,JCIH  ,HH    ,Z     ,P     ,
     4                    R     ,Q     ,LTSM  ,HL    ,DIAG_H,
     5                    DIAG_L,WORK1 ,WORK2 ,WORK3 ,LAMBDA,
     6                    RBYL  ,NPBYL ,AR    ,VR    ,IN    ,
     7                    IADHF ,JCIHF ,ICFTAG,JCFTAG,NCF_S ,
     8                    NCF_E )
C-----------------------------------------------
C   I m p l i c i t   T y p e s
C-----------------------------------------------
#include "implicit_f.inc"
C-----------------------------------------------
C   C o m m o n   B l o c k s
C-----------------------------------------------
#include      "param_c.inc"
#include      "lagmult.inc"
#include      "com08_c.inc"
#include      "units_c.inc"
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      INTEGER NC,NCR,NCF_S,NCF_E,NH 
      INTEGER LLL(*),JLL(*),IADLL(*),IADH(*),JCIH(*),IADHF(*),
     .        JCIHF(*),NPBYL(NNPBY,*),WORK2(*),WORK3(*),
     .        ICFTAG(*),JCFTAG(*)
C     REAL
      my_real
     .  MAS(*),IN(*), A(3,*),AR(3,*),V(3,*),VR(3,*),
     .  XLL(*),P(*),R(*),Q(*),LAMBDA(*),RBYL(NRBY,*),
     .  ltsm(6,*),z(*),hh(*),hl(*),diag_h(*),diag_l(*),
     .  work1(*)
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      INTEGER I,J,IK,IC,JC,IH,IP,ITER,NITER,ITERMAX,NIN,LENH
      my_real
     .   AS,ASMAX,ALPHA,BETA,R2,R2NEW,RNORM,PQ,HIJ,SCALE,DD
C-----------------------------------------------
C    External Functions
C-----------------------------------------------
      INTEGER  CHOLFACT
      EXTERNAL CHOLFACT
C-----------------------------------------------
C
C       | M | Lt| | a    | M ao
C       |---+---| |    = |
C       | L | 0 | | la   | bo
C
C        [M] a + [L]t la = [M] ao
C        [L] a = bo
C
C         a = -[M]-1[L]t la + ao
C        [L][M]-1[L]t la  = [L] ao - bo
C
C        [H] = [L][M]-1[L]t
C        b  = [L] ao - bo
C        [H] la = b
C
C        a = ao - [M]-1[L]t la
C-----------------------------------------------
C
C        la              : LAMBDA(NC)
C        ao              : A(3,NUMNOD)
C        [M](i)          : MAS(NUMNOD)
C        [L][M]-1[L]t la : Q(NC)
C        [L] ao - b      : B(NC)
C        [M]-1[L]t       : LTSM(3,NUMNOD) une colonne de [M]-1[L]t
C                        : Z(NC)
C
C        [L](ic,j,i)    -> XLL(IK) 
C        ic = 1..nc      : IK = IADLL(IC)...IADLL(IC+1)-1
C        j  = 1,2,3      : J  = JLL(IK)
C        i  = 1..numnod  : I  = LLL(IK)
C
C
C        NC : nombre de contact 
C
C        IC : numero du contact (1,NC)
C        IK : index de XLL,LLL,JLL
C        I  : numero global du noeud (1,NUMNOD)
C        J  : direction 1,2,3
C
C-----------------------------------------------
C        [H](ic,jc)     -> HH(IH)
C        ic = 1..nc      : IH = IADH(IC)...IADH(IC+1)-1
C        jc = 1..ic      : JC = JCIH(IH)
C
C        q = [H] p
C     
C        do ic=1,nc
C          q(ic) = 0
C          do jc=1,nc
C            q(ic) = q(ic) + h(ic,jc)*p(jc)
C          enddo
C        enddo
C
C        DO IC=1,NC
C          Q(IC) = 0
C          DO IH=IADH(IC),IADH(IC+1)-1
C            Q(IC) = Q(IC) - HH(IH)*P(JCIH(IH))
C          ENDDO
C        ENDDO
C-----------------------------------------------
C  evaluation de b:
C
C         Vs = Somme(Ni Vi) + vout
C         Vs_ + dt As = Somme(Ni Vi_) + Somme(dt Ni Ai) + vout
C         Somme(dt Ni Ai) - dt As =  Vs_ -Somme(Ni Vi_) - vout
C         [L] = dt {N1,N2,..,N15,-1}
C         bo = {N1,N2,..,N15,-1} a = -[L]/dt v_ - vout
C         b  = [L] ao - bo
C         b  = [L] ao + [L]/dt v_  + vout
C         b  = [L]/dt vo+  + vout
C-----------------------------------------------------------------------
C
C     calcul de [H] la = b par gradient conjugue
C
C-----------------------------------------------------------------------
C      Gradient conjugue [H] la = b avec preconditionnement [N]
C      [H]-1 = Somme([I-H]^i),i=0,1...inf    si |I-H|<1
C      [N]-1 = [2I-H]
C-----------------------------------------------------------------------
C      r(0) = b - [H] la(0)
C      for i=1,niter
C        z(i-1) = [N]-1 r(i-1)
C        rho(i-1) = r(i-1)T z(i-1)
C        if (i == 1)
C          p(1) = z(0)
C        else
C          beta = rho(i-1) / rho(i-2)
C          p(i) = z(i-1) + beta p(i-1)
C        endif
C        q(i)  = [H] p(i)
C        alpha = rho(i-1) / p(i)T q(i)
C        la(i) = la(i-1) + alpha p(i)
C        r(i)  = r(i-1)  - alpha q(i)
C     end
C======================================================================|
      scale = three_over_4
      asmax = zep6
      r2new = zero      
      lag_nc= nc
C---
C---  Calcul de la matrice H
      CALL lag_mult_h(nc     ,lenh   ,nh     ,mas    ,in     ,
     2                diag_h ,hh     ,iadll  ,lll    ,jll    ,
     3                xll    ,ltsm   ,iadhf  ,jcihf  ,iadh   ,
     4                jcih   ,rbyl   ,npbyl  ,icftag ,jcftag ,
     5                ncf_s  ,ncf_e  ,ncr    )
C------------------------------------------
C     Normalisation du systeme
c     lagopt=0 => sans norme
c     lagopt=1 => norme L1 (diagonal)
c     lagopt=2 => norme L2 (columns)
C------------------------------------------
      IF (lagopt==1) THEN
        dd = zero
        DO ic=1,nc
          dd = dd + diag_h(ic)*diag_h(ic)
        ENDDO
        DO ic=1,nc
          z(ic) = dd
        ENDDO
      ELSEIF (lagopt==2) THEN
        DO ic=1,nc
          z(ic) = diag_h(ic)*diag_h(ic)
        ENDDO
        DO ic=1,nc
          DO ih=iadh(ic),iadh(ic+1)-1
            jc  = jcih(ih)
            hij = hh(ih)*hh(ih)
            z(ic) = z(ic) + hij
            z(jc) = z(jc) + hij
          ENDDO
        ENDDO
      ENDIF
C---
      IF (lagopt>0) THEN
        DO ic=1,nc
          z(ic) = scale/sqrt(sqrt(z(ic)))
        ENDDO
C---      normalisation [H]        Somme([H](ic,i)^2)=1   i=1...nc
        DO ic=1,nc
          diag_h(ic) = diag_h(ic)*z(ic)*z(ic)
          DO ih=iadh(ic),iadh(ic+1)-1
            hh(ih) = hh(ih)*z(ic)*z(jcih(ih))
          ENDDO
        ENDDO
C---      normalisation [LLL] et {R}
        DO ic=1,nc
          DO ik=iadll(ic),iadll(ic+1)-1
            xll(ik)  = xll(ik)*z(ic)
          ENDDO
          r(ic) = r(ic)*z(ic)
        ENDDO
      ENDIF
C------------------------------------------
C     Factorisation de [H] : Cholesky incomplet + shift iteratif
C------------------------------------------
      niter_chl  = 0
      IF (lagmod==1) THEN
        alpha = lag_alph
        IF (lag_alphs==zero) THEN
          DO ic=1,nc
            diag_l(ic) =  diag_h(ic)
          ENDDO
        ELSE
          DO ic=1,nc
            diag_l(ic) =  diag_h(ic) + lag_alphs
          ENDDO
        ENDIF
        ip    = 1
        DO WHILE (ip>0)
          DO ih=1,lenh
            hl(ih) =  hh(ih)
          ENDDO
          ip = cholfact(nc,diag_l,hl,iadh,jcih,work1,work2,work3)
          niter_chl  = niter_chl+1  
          alpha = alpha*2
          as    = alpha + lag_alphs
          IF (as>asmax) THEN
            ip = 0
            WRITE(iout,*) '***WARNING (LAGMULT : FACTORISATION FAILED )'
            WRITE(istdo,*)'***WARNING (LAGMULT : FACTORISATION FAILED )'
          ELSEIF (ip>0) THEN
            DO ic=1,nc
              diag_l(ic) =  diag_h(ic) + alpha + lag_alphs
            ENDDO
          ENDIF
        ENDDO
        IF (niter_chl>1) print*,'FACTOR ITERATIONS = ',niter_chl
       ENDIF
C=======================================================================
C     calcul de  b (r=b)
C=======================================================================
      DO ic=1,nc
        DO ik=iadll(ic),iadll(ic+1)-1
          i = lll(ik)
          j = jll(ik)
          IF (j>3) THEN
            j = j-3
            r(ic) = r(ic) + xll(ik)*(vr(j,i)/dt12+ar(j,i))
          ELSE
            r(ic) = r(ic) + xll(ik)*(v(j,i)/dt12+a(j,i))
          ENDIF
        ENDDO
      ENDDO
C---  Initialisation du vecteur solution
      rnorm = zero
      DO ic=1,nc
        rnorm = rnorm + r(ic)*r(ic)
        p(ic) = lambda(ic)
      ENDDO
C---  Residu initial   R(i) = R(i) - sum(H(i,j)*P(j))
      DO ic=1,nc
        r(ic) = r(ic) - diag_h(ic)*p(ic)
        DO ih=iadh(ic),iadh(ic+1)-1
          jc = jcih(ih)
          r(ic) = r(ic) - hh(ih)*p(jc)
          r(jc) = r(jc) - hh(ih)*p(ic)
        ENDDO
      ENDDO
C=======================================================================
C     iterations
C=======================================================================
      itermax = nc
      iter    = 0
      niter   = 0
      DO WHILE(iter<itermax)
        iter  = iter+1
        niter = iter
C--------------------
C       preconditionnement  z = inv(H) r
C--------------------
        IF (lagmod==1) THEN
C---    preconditionnement de Cholesky
          CALL prechol(z,diag_l,hl,r,nc,iadh,jcih)
        ELSEIF(lagmod==2) THEN
C---    preconditionnement polynomial :  H=I-B,  z = [I+B] r    
          DO ic=1,nc
            z(ic) = r(ic) + r(ic)
          ENDDO
          DO ic=1,nc
            z(ic) = z(ic) - diag_h(ic)*r(ic)
            DO ih=iadh(ic),iadh(ic+1)-1
              jc = jcih(ih)
              z(ic) = z(ic) - hh(ih)*r(jc)
              z(jc) = z(jc) - hh(ih)*r(ic)
            ENDDO
          ENDDO
        ENDIF
C--------------------
        r2    = r2new
        r2new = zero
        DO ic=1,nc
          r2new = r2new + r(ic)*z(ic)
          q(ic) = zero
        ENDDO
C--------------------
        IF(iter==1)THEN
          DO ic=1,nc
            p(ic) = z(ic)
          ENDDO
        ELSE
          beta = r2new/r2
          DO ic=1,nc
            p(ic) = z(ic) + beta*p(ic)
          ENDDO
        ENDIF
C-----------------------------------------------------------------------
C     calcul de  q = q_ + [H] p
C-----------------------------------------------------------------------
      DO ic=1,nc
        q(ic) = q(ic) + diag_h(ic)*p(ic)
        DO ih=iadh(ic),iadh(ic+1)-1
          jc = jcih(ih)
          q(ic) = q(ic) + hh(ih)*p(jc)          
          q(jc) = q(jc) + hh(ih)*p(ic)          
        ENDDO
      ENDDO
C-----------------------------------------------------------------------
C     pt [H] p = pt q
C-----------------------------------------------------------------------
        pq = zero
        DO ic=1,nc
          pq = pq + p(ic)*q(ic)
        ENDDO
C-----------------------------------------------------------------------
        IF(pq==zero)THEN
          iter = itermax
        ELSE
          alpha = r2new/pq
          lag_ersq2 = zero
          DO ic=1,nc
            lambda(ic) = lambda(ic) + alpha*p(ic)
            r(ic)      = r(ic)      - alpha*q(ic)
            lag_ersq2  = lag_ersq2  + r(ic)*r(ic)
          ENDDO
C--------- test convergence --------------------------------------------
          lag_ersq2 = lag_ersq2/max(em30,rnorm)
          IF(lag_ersq2<lagm_tol)THEN
            iter = itermax
          ENDIF
        ENDIF
C=======================================================================
C     fin boucle gradient conjugue
C=======================================================================
      ENDDO
      niter_gc = niter
C---
C     a = ao + [M]-1[L]t la  
C---
      DO ic=1,ncr
        DO ik=iadll(ic),iadll(ic+1)-1
          i = lll(ik)
          j = jll(ik)
          xll(ik) = xll(ik)*lambda(ic)
          IF(j>3) THEN
            j = j-3
            ar(j,i) = ar(j,i) - xll(ik)/in(i)
          ELSE         
            a(j,i)  = a(j,i)  - xll(ik)/mas(i)
          ENDIF         
        ENDDO
      ENDDO
C--------------------------------------------
      RETURN
      END
C
!||====================================================================
!||    lag_mult_solvp   ../engine/source/tools/lagmul/lag_mult_solv.F
!||--- called by ------------------------------------------------------
!||    lag_multp        ../engine/source/tools/lagmul/lag_mult.F
!||--- calls      -----------------------------------------------------
!||    cholfact         ../engine/source/tools/lagmul/cholfact.F
!||    lag_mult_hp      ../engine/source/tools/lagmul/lag_mult_h.F
!||    prechol          ../engine/source/tools/lagmul/cholfact.F
!||====================================================================
      SUBROUTINE lag_mult_solvp(
     1                    NH    ,NC      ,NCR   ,A     ,V     ,
     2                    MAS   ,IADLL   ,LLL   ,JLL   ,XLL   ,
     3                    IADH  ,JCIH    ,HH    ,Z     ,P     ,
     4                    R     ,Q       ,LTSM  ,HL    ,DIAG_H,
     5                    DIAG_L,WORK1   ,WORK2 ,WORK3 ,LAMBDA,
     6                    RBYL  ,NPBYL   ,AR    ,VR    ,IN    ,
     7                    IADHF ,JCIHF   ,ICFTAG,JCFTAG,NCF_S ,
     8                    NCF_E ,INDEXLAG)
C-----------------------------------------------
C   I m p l i c i t   T y p e s
C-----------------------------------------------
#include "implicit_f.inc"
C-----------------------------------------------
C   C o m m o n   B l o c k s
C-----------------------------------------------
#include      "param_c.inc"
#include      "lagmult.inc"
#include      "com08_c.inc"
#include      "units_c.inc"
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      INTEGER NC,NCR,NCF_S,NCF_E,NH 
      INTEGER LLL(*),JLL(*),IADLL(*),IADH(*),JCIH(*),IADHF(*),
     .        JCIHF(*),NPBYL(NNPBY,*),WORK2(*),WORK3(*),
     .        ICFTAG(*),JCFTAG(*), INDEXLAG(*)
C     REAL
      my_real
     .  MAS(*),IN(*), A(3,*),AR(3,*),V(3,*),VR(3,*),
     .  XLL(*),P(*),R(*),Q(*),LAMBDA(*),RBYL(NRBY,*),
     .  LTSM(6,*),Z(*),HH(*),HL(*),DIAG_H(*),DIAG_L(*),
     .  WORK1(*)
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      INTEGER I,J,IK,IC,JC,IH,IP,ITER,NITER,ITERMAX,NIN,LENH,
     .        NZ
      my_real
     .   as,asmax,alpha,beta,r2,r2new,rnorm,pq,hij,scale,dd
C-----------------------------------------------
C    External Functions
C-----------------------------------------------
      INTEGER  CHOLFACT
      EXTERNAL cholfact
C-----------------------------------------------
C
C       | M | Lt| | a    | M ao
C       |---+---| |    = |
C       | L | 0 | | la   | bo
C
C        [M] a + [L]t la = [M] ao
C        [L] a = bo
C
C         a = -[M]-1[L]t la + ao
C        [L][M]-1[L]t la  = [L] ao - bo
C
C        [H] = [L][M]-1[L]t
C        b  = [L] ao - bo
C        [H] la = b
C
C        a = ao - [M]-1[L]t la
C-----------------------------------------------
C
C        la              : LAMBDA(NC)
C        ao              : A(3,NUMNOD)
C        [M](i)          : MAS(NUMNOD)
C        [L][M]-1[L]t la : Q(NC)
C        [L] ao - b      : B(NC)
C        [M]-1[L]t       : LTSM(3,NUMNOD) une colonne de [M]-1[L]t
C                        : Z(NC)
C
C        [L](ic,j,i)    -> XLL(IK) 
C        ic = 1..nc      : IK = IADLL(IC)...IADLL(IC+1)-1
C        j  = 1,2,3      : J  = JLL(IK)
C        i  = 1..numnod  : I  = LLL(IK)
C
C
C        NC : nombre de contact 
C
C        IC : numero du contact (1,NC)
C        IK : index de XLL,LLL,JLL
C        I  : numero global du noeud (1,NUMNOD)
C        J  : direction 1,2,3
C
C-----------------------------------------------
C        [H](ic,jc)     -> HH(IH)
C        ic = 1..nc      : IH = IADH(IC)...IADH(IC+1)-1
C        jc = 1..ic      : JC = JCIH(IH)
C
C        q = [H] p
C     
C        do ic=1,nc
C          q(ic) = 0
C          do jc=1,nc
C            q(ic) = q(ic) + h(ic,jc)*p(jc)
C          enddo
C        enddo
C
C        DO IC=1,NC
C          Q(IC) = 0
C          DO IH=IADH(IC),IADH(IC+1)-1
C            Q(IC) = Q(IC) - HH(IH)*P(JCIH(IH))
C          ENDDO
C        ENDDO
C-----------------------------------------------
C  evaluation de b:
C
C         Vs = Somme(Ni Vi) + vout
C         Vs_ + dt As = Somme(Ni Vi_) + Somme(dt Ni Ai) + vout
C         Somme(dt Ni Ai) - dt As =  Vs_ -Somme(Ni Vi_) - vout
C         [L] = dt {N1,N2,..,N15,-1}
C         bo = {N1,N2,..,N15,-1} a = -[L]/dt v_ - vout
C         b  = [L] ao - bo
C         b  = [L] ao + [L]/dt v_  + vout
C         b  = [L]/dt vo+  + vout
C-----------------------------------------------------------------------
C
C     calcul de [H] la = b par gradient conjugue
C
C-----------------------------------------------------------------------
C      Gradient conjugue [H] la = b avec preconditionnement [N]
C      [H]-1 = Somme([I-H]^i),i=0,1...inf    si |I-H|<1
C      [N]-1 = [2I-H]
C-----------------------------------------------------------------------
C      r(0) = b - [H] la(0)
C      for i=1,niter
C        z(i-1) = [N]-1 r(i-1)
C        rho(i-1) = r(i-1)T z(i-1)
C        if (i == 1)
C          p(1) = z(0)
C        else
C          beta = rho(i-1) / rho(i-2)
C          p(i) = z(i-1) + beta p(i-1)
C        endif
C        q(i)  = [H] p(i)
C        alpha = rho(i-1) / p(i)T q(i)
C        la(i) = la(i-1) + alpha p(i)
C        r(i)  = r(i-1)  - alpha q(i)
C     end
C======================================================================|
      scale = three_over_4
      asmax = zep6
      r2new = zero      
      lag_nc= nc
C---
C---  Calcul de la matrice H
      CALL lag_mult_hp(nc     ,lenh   ,nh     ,mas     ,in     ,
     2                 diag_h ,hh     ,iadll  ,lll     ,jll    ,
     3                 xll    ,ltsm   ,iadhf  ,jcihf   ,iadh   ,
     4                 jcih   ,rbyl   ,npbyl  ,icftag  ,jcftag ,
     5                 ncf_s  ,ncf_e  ,ncr    ,indexlag)
C------------------------------------------
C     Normalisation du systeme
c     lagopt=0 => sans norme
c     lagopt=1 => norme L1 (diagonal)
c     lagopt=2 => norme L2 (columns)
C------------------------------------------
      IF (lagopt==1) THEN
        dd = zero
        DO ic=1,nc
          dd = dd + diag_h(ic)*diag_h(ic)
        ENDDO
        DO ic=1,nc
          z(ic) = dd
        ENDDO
      ELSEIF (lagopt==2) THEN
        DO ic=1,nc
          z(ic) = diag_h(ic)*diag_h(ic)
        ENDDO
        DO ic=1,nc
          DO ih=iadh(ic),iadh(ic+1)-1
            jc  = jcih(ih)
            hij = hh(ih)*hh(ih)
            z(ic) = z(ic) + hij
            z(jc) = z(jc) + hij
          ENDDO
        ENDDO
      ENDIF
C---
      IF (lagopt>0) THEN
        DO ic=1,nc
          z(ic) = scale/sqrt(sqrt(z(ic)))
        ENDDO
C---      normalisation [H]        Somme([H](ic,i)^2)=1   i=1...nc
        DO ic=1,nc
          diag_h(ic) = diag_h(ic)*z(ic)*z(ic)
          DO ih=iadh(ic),iadh(ic+1)-1
            hh(ih) = hh(ih)*z(ic)*z(jcih(ih))
          ENDDO
        ENDDO
C---      normalisation [LLL] et {R}
        DO ic=1,nc
          DO ik=iadll(ic),iadll(ic+1)-1
            xll(ik)  = xll(ik)*z(ic)
          ENDDO
          r(ic) = r(ic)*z(ic)
        ENDDO
      ENDIF
C------------------------------------------
C     Factorisation de [H] : Cholesky incomplet + shift iteratif
C------------------------------------------
      niter_chl  = 0
      IF (lagmod==1) THEN
        alpha = lag_alph
        IF (lag_alphs==zero) THEN
          DO ic=1,nc
            diag_l(ic) =  diag_h(ic)
          ENDDO
        ELSE
          DO ic=1,nc
            diag_l(ic) =  diag_h(ic) + lag_alphs
          ENDDO
        ENDIF
        ip    = 1
        DO WHILE (ip>0)
          DO ih=1,lenh
            hl(ih) =  hh(ih)
          ENDDO
          ip = cholfact(nc,diag_l,hl,iadh,jcih,work1,work2,work3)
          niter_chl  = niter_chl+1  
          alpha = alpha*2
          as    = alpha + lag_alphs
          IF (as>asmax) THEN
            ip = 0
            WRITE(iout,*) '***WARNING (LAGMULT : FACTORISATION FAILED )'
            WRITE(istdo,*)'***WARNING (LAGMULT : FACTORISATION FAILED )'
          ELSEIF (ip>0) THEN
            DO ic=1,nc
              diag_l(ic) =  diag_h(ic) + alpha + lag_alphs
            ENDDO
          ENDIF
        ENDDO
        IF (niter_chl>1) print*,'FACTOR ITERATIONS = ',niter_chl
       ENDIF
C=======================================================================
C     calcul de  b (r=b)
C=======================================================================
      DO ic=1,nc
        DO ik=iadll(ic),iadll(ic+1)-1
          i = indexlag(lll(ik))
          j = jll(ik)
          IF (j>3) THEN
            j = j-3
            r(ic) = r(ic) + xll(ik)*(vr(j,i)/dt12+ar(j,i))
          ELSE
            r(ic) = r(ic) + xll(ik)*(v(j,i)/dt12+a(j,i))
          ENDIF
        ENDDO
      ENDDO
C---  Initialisation du vecteur solution
      rnorm = zero
      DO ic=1,nc
        rnorm = rnorm + r(ic)*r(ic)
        p(ic) = lambda(ic)
      ENDDO
C---  Residu initial   R(i) = R(i) - sum(H(i,j)*P(j))
      DO ic=1,nc
        r(ic) = r(ic) - diag_h(ic)*p(ic)
        DO ih=iadh(ic),iadh(ic+1)-1
          jc = jcih(ih)
          r(ic) = r(ic) - hh(ih)*p(jc)
          r(jc) = r(jc) - hh(ih)*p(ic)
        ENDDO
      ENDDO
C=======================================================================
C     iterations
C=======================================================================
      itermax = nc
      iter    = 0
      DO WHILE(iter<itermax)
        iter  = iter+1
        niter = iter
C--------------------
C       preconditionnement  z = inv(H) r
C--------------------
        IF (lagmod==1) THEN
C---    preconditionnement de Cholesky
          CALL prechol(z,diag_l,hl,r,nc,iadh,jcih)
        ELSEIF(lagmod==2) THEN
C---    preconditionnement polynomial :  H=I-B,  z = [I+B] r    
          DO ic=1,nc
            z(ic) = r(ic) + r(ic)
          ENDDO
          DO ic=1,nc
            z(ic) = z(ic) - diag_h(ic)*r(ic)
            DO ih=iadh(ic),iadh(ic+1)-1
              jc = jcih(ih)
              z(ic) = z(ic) - hh(ih)*r(jc)
              z(jc) = z(jc) - hh(ih)*r(ic)
            ENDDO
          ENDDO
        ENDIF
C--------------------
        r2    = r2new
        r2new = zero
        DO ic=1,nc
          r2new = r2new + r(ic)*z(ic)
          q(ic) = zero
        ENDDO
C--------------------
        IF(iter==1)THEN
          DO ic=1,nc
            p(ic) = z(ic)
          ENDDO
        ELSE
          beta = r2new/r2
          DO ic=1,nc
            p(ic) = z(ic) + beta*p(ic)
          ENDDO
        ENDIF
C-----------------------------------------------------------------------
C     calcul de  q = q_ + [H] p
C-----------------------------------------------------------------------
      DO ic=1,nc
        q(ic) = q(ic) + diag_h(ic)*p(ic)
        DO ih=iadh(ic),iadh(ic+1)-1
          jc = jcih(ih)
          q(ic) = q(ic) + hh(ih)*p(jc)          
          q(jc) = q(jc) + hh(ih)*p(ic)          
        ENDDO
      ENDDO
C-----------------------------------------------------------------------
C     pt [H] p = pt q
C-----------------------------------------------------------------------
        pq = zero
        DO ic=1,nc
          pq = pq + p(ic)*q(ic)
        ENDDO
C-----------------------------------------------------------------------
        IF(pq==zero)THEN
          iter = itermax
        ELSE
          alpha = r2new/pq
          lag_ersq2 = zero
          DO ic=1,nc
            lambda(ic) = lambda(ic) + alpha*p(ic)
            r(ic)      = r(ic)      - alpha*q(ic)
            lag_ersq2  = lag_ersq2  + r(ic)*r(ic)
          ENDDO
C--------- test convergence --------------------------------------------
          lag_ersq2 = lag_ersq2/max(em30,rnorm)
          IF(lag_ersq2<lagm_tol)THEN
            iter = itermax
          ENDIF
        ENDIF
C=======================================================================
C     fin boucle gradient conjugue
C=======================================================================
      ENDDO
      niter_gc = niter
C---
C     a = ao + [M]-1[L]t la  
C---
      DO ic=1,ncr
        DO ik=iadll(ic),iadll(ic+1)-1
          i = indexlag(lll(ik))
          j = jll(ik)
          xll(ik) = xll(ik)*lambda(ic)
          IF(j>3) THEN
            j = j-3
            ar(j,i) = ar(j,i) - xll(ik)/in(i)
          ELSE         
            a(j,i)  = a(j,i)  - xll(ik)/mas(i)
          ENDIF         
        ENDDO
      ENDDO
C--------------------------------------------
      RETURN
      END
C
!||====================================================================
!||    lag_mult_sdp      ../engine/source/tools/lagmul/lag_mult_solv.F
!||--- calls      -----------------------------------------------------
!||    arret             ../engine/source/system/arret.F
!||    cholfact          ../engine/source/tools/lagmul/cholfact.F
!||    lag_mult_hp       ../engine/source/tools/lagmul/lag_mult_h.F
!||    spmd_mumps_deal   ../engine/source/mpi/implicit/imp_spmd.F
!||    spmd_mumps_exec   ../engine/source/mpi/implicit/imp_spmd.F
!||    spmd_mumps_ini    ../engine/source/mpi/implicit/imp_spmd.F
!||====================================================================
      SUBROUTINE lag_mult_sdp(
     1                    NH    ,NC      ,NCR   ,A     ,V     ,
     2                    MAS   ,IADLL   ,LLL   ,JLL   ,XLL   ,
     3                    IADH  ,JCIH    ,HH    ,Z     ,P     ,
     4                    R     ,Q       ,LTSM  ,HL    ,DIAG_H,
     5                    DIAG_L,WORK1   ,WORK2 ,WORK3 ,LAMBDA,
     6                    RBYL  ,NPBYL   ,AR    ,VR    ,IN    ,
     7                    IADHF ,JCIHF   ,ICFTAG,JCFTAG,NCF_S ,
     8                    NCF_E ,INDEXLAG)
C-----------------------------------------------
C   I m p l i c i t   T y p e s
C-----------------------------------------------
#include "implicit_f.inc"
C-----------------------------------------------
C   C o m m o n   B l o c k s
C-----------------------------------------------
#include "param_c.inc"
#include "task_c.inc"
#include "com08_c.inc"
#ifdef MUMPS5
#include "dmumps_struc.h"
#include "lagmult.inc"
#endif
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      INTEGER NC,NCR,NCF_S,NCF_E,NH 
      INTEGER LLL(*),JLL(*),IADLL(*),IADH(*),JCIH(*),IADHF(*),
     .        JCIHF(*),NPBYL(NNPBY,*),WORK2(*),WORK3(*),
     .        ICFTAG(*),JCFTAG(*), INDEXLAG(*)
      my_real
     .  MAS(*),IN(*), A(3,*),AR(3,*),V(3,*),VR(3,*),
     .  XLL(*),P(*),R(*),Q(*),LAMBDA(*),RBYL(NRBY,*),
     .  LTSM(6,*),Z(*),HH(*),HL(*),DIAG_H(*),DIAG_L(*),
     .  WORK1(*)
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      INTEGER I,J,IK,IC,JC,IH,IP,ITER,NITER,ITERMAX,NIN,LENH,NZ
      my_real
     .   AS,ASMAX,ALPHA,BETA,R2,R2NEW,RNORM,PQ,HIJ,SCALE,DD
#ifdef MUMPS5
      type(dmumps_struc)::MUMPS_PAR
#endif
C-----------------------------------------------
C    External Functions
C-----------------------------------------------
      INTEGER  CHOLFACT
      EXTERNAL CHOLFACT
C-----------------------------------------------
C
C       | M | Lt| | a    | M ao
C       |---+---| |    = |
C       | L | 0 | | la   | bo
C
C        [M] a + [L]t la = [M] ao
C        [L] a = bo
C
C         a = -[M]-1[L]t la + ao
C        [L][M]-1[L]t la  = [L] ao - bo
C
C        [H] = [L][M]-1[L]t
C        b  = [L] ao - bo
C        [H] la = b
C
C        a = ao - [M]-1[L]t la
C-----------------------------------------------
C
C        la              : LAMBDA(NC)
C        ao              : A(3,NUMNOD)
C        [M](i)          : MAS(NUMNOD)
C        [L][M]-1[L]t la : Q(NC)
C        [L] ao - b      : B(NC)
C        [M]-1[L]t       : LTSM(3,NUMNOD) une colonne de [M]-1[L]t
C                        : Z(NC)
C
C        [L](ic,j,i)    -> XLL(IK) 
C        ic = 1..nc      : IK = IADLL(IC)...IADLL(IC+1)-1
C        j  = 1,2,3      : J  = JLL(IK)
C        i  = 1..numnod  : I  = LLL(IK)
C
C
C        NC : nombre de contact 
C
C        IC : numero du contact (1,NC)
C        IK : index de XLL,LLL,JLL
C        I  : numero global du noeud (1,NUMNOD)
C        J  : direction 1,2,3
C
C-----------------------------------------------
C        [H](ic,jc)     -> HH(IH)
C        ic = 1..nc      : IH = IADH(IC)...IADH(IC+1)-1
C        jc = 1..ic      : JC = JCIH(IH)
C
C        q = [H] p
C     
C        do ic=1,nc
C          q(ic) = 0
C          do jc=1,nc
C            q(ic) = q(ic) + h(ic,jc)*p(jc)
C          enddo
C        enddo
C
C        DO IC=1,NC
C          Q(IC) = 0
C          DO IH=IADH(IC),IADH(IC+1)-1
C            Q(IC) = Q(IC) - HH(IH)*P(JCIH(IH))
C          ENDDO
C        ENDDO
C-----------------------------------------------
C  evaluation de b:
C
C         Vs = Somme(Ni Vi) + vout
C         Vs_ + dt As = Somme(Ni Vi_) + Somme(dt Ni Ai) + vout
C         Somme(dt Ni Ai) - dt As =  Vs_ -Somme(Ni Vi_) - vout
C         [L] = dt {N1,N2,..,N15,-1}
C         bo = {N1,N2,..,N15,-1} a = -[L]/dt v_ - vout
C         b  = [L] ao - bo
C         b  = [L] ao + [L]/dt v_  + vout
C         b  = [L]/dt vo+  + vout
C-----------------------------------------------------------------------
C
C     calcul de [H] la = b par gradient conjugue
C
C-----------------------------------------------------------------------
C      Gradient conjugue [H] la = b avec preconditionnement [N]
C      [H]-1 = Somme([I-H]^i),i=0,1...inf    si |I-H|<1
C      [N]-1 = [2I-H]
C-----------------------------------------------------------------------
C      r(0) = b - [H] la(0)
C      for i=1,niter
C        z(i-1) = [N]-1 r(i-1)
C        rho(i-1) = r(i-1)T z(i-1)
C        if (i == 1)
C          p(1) = z(0)
C        else
C          beta = rho(i-1) / rho(i-2)
C          p(i) = z(i-1) + beta p(i-1)
C        endif
C        q(i)  = [H] p(i)
C        alpha = rho(i-1) / p(i)T q(i)
C        la(i) = la(i-1) + alpha p(i)
C        r(i)  = r(i-1)  - alpha q(i)
C     end
C======================================================================|
#ifdef MUMPS5
      IF(ispmd==0) THEN
       scale = three_over_4
       asmax = zep6
       r2new = zero      
       lag_nc= nc
C---
C---  Calcul de la matrice H
       CALL lag_mult_hp(nc     ,lenh   ,nh     ,mas     ,in     ,
     2                  diag_h ,hh     ,iadll  ,lll     ,jll    ,
     3                  xll    ,ltsm   ,iadhf  ,jcihf   ,iadh   ,
     4                  jcih   ,rbyl   ,npbyl  ,icftag  ,jcftag ,
     5                  ncf_s  ,ncf_e  ,ncr    ,indexlag)
C------------------------------------------
C     Normalisation du systeme
c     lagopt=0 => sans norme
c     lagopt=1 => norme L1 (diagonal)
c     lagopt=2 => norme L2 (columns)
C------------------------------------------
       IF (lagopt==1) THEN
        dd = zero
        DO ic=1,nc
          dd = dd + diag_h(ic)*diag_h(ic)
        ENDDO
        DO ic=1,nc
          z(ic) = dd
        ENDDO
       ELSEIF (lagopt==2) THEN
        DO ic=1,nc
          z(ic) = diag_h(ic)*diag_h(ic)
        ENDDO
        DO ic=1,nc
          DO ih=iadh(ic),iadh(ic+1)-1
            jc  = jcih(ih)
            hij = hh(ih)*hh(ih)
            z(ic) = z(ic) + hij
            z(jc) = z(jc) + hij
          ENDDO
        ENDDO
       ENDIF
C---
       IF (lagopt>0) THEN
        DO ic=1,nc
          z(ic) = scale/sqrt(sqrt(z(ic)))
        ENDDO
C---      normalisation [H]        Somme([H](ic,i)^2)=1   i=1...nc
        DO ic=1,nc
          diag_h(ic) = diag_h(ic)*z(ic)*z(ic)
          DO ih=iadh(ic),iadh(ic+1)-1
            hh(ih) = hh(ih)*z(ic)*z(jcih(ih))
          ENDDO
        ENDDO
C---      normalisation [LLL] et {R}
        DO ic=1,nc
          DO ik=iadll(ic),iadll(ic+1)-1
            xll(ik)  = xll(ik)*z(ic)
          ENDDO
          r(ic) = r(ic)*z(ic)
        ENDDO
       ENDIF
C------------------------------------------
C     Factorisation de [H] : Cholesky incomplet + shift iteratif
C------------------------------------------
C      NITER_CHL  = 0
C      IF (LAGMOD==1) THEN
C        ALPHA = LAG_ALPH
C        IF (LAG_ALPHS==ZERO) THEN
C          DO IC=1,NC
C            DIAG_L(IC) =  DIAG_H(IC)
C          ENDDO
C        ELSE
C          DO IC=1,NC
C            DIAG_L(IC) =  DIAG_H(IC) + LAG_ALPHS
C          ENDDO
C        ENDIF
C        IP    = 1
C        DO WHILE (IP>0)
C          DO IH=1,LENH
C            HL(IH) =  HH(IH)
C          ENDDO
C          IP = CHOLFACT(NC,DIAG_L,HL,IADH,JCIH,WORK1,WORK2,WORK3)
C          NITER_CHL  = NITER_CHL+1  
C          ALPHA = ALPHA*2
C          AS    = ALPHA + LAG_ALPHS
C          IF (AS>ASMAX) THEN
C            IP = 0
C            WRITE(IOUT,*) '***WARNING (LAGMULT : FACTORISATION FAILED )'
C            WRITE(ISTDO,*)'***WARNING (LAGMULT : FACTORISATION FAILED )'
C          ELSEIF (IP>0) THEN
C            DO IC=1,NC
C              DIAG_L(IC) =  DIAG_H(IC) + ALPHA + LAG_ALPHS
C            ENDDO
C          ENDIF
C        ENDDO
C        IF (NITER_CHL>1) print*,'FACTOR ITERATIONS = ',NITER_CHL
C       ENDIF
      END IF ! fin proc0
C=======================================================================
C     MUMPS
C=======================================================================
C
C Init structure
C
      CALL spmd_mumps_ini(mumps_par,1)
      mumps_par%ICNTL(18) = 0            ! donnees centralisees sur p0
C flag output
      mumps_par%ICNTL(3)=-1              ! ou iout par exemple si sorties voulues
      IF(ispmd==0) THEN ! structure de stockage sur p0
        mumps_par%N         = nc               ! valid sur proc0 uniquement
        mumps_par%NZ        = nc+iadh(nc+1)-1     ! valid sur proc0 uniquement
        ALLOCATE(mumps_par%A(mumps_par%NZ))
        ALLOCATE(mumps_par%IRN(mumps_par%NZ))
        ALLOCATE(mumps_par%JCN(mumps_par%NZ))
        DO ic = 1, nc
          mumps_par%A(ic) = diag_h(ic)
          mumps_par%IRN(ic)=ic
          mumps_par%JCN(ic)=ic
        END DO
        nz = nc
        DO ic = 1, nc
          DO ih=iadh(ic),iadh(ic+1)-1
            nz = nz + 1
            mumps_par%A(nz) = hh(ih)
            mumps_par%IRN(nz)=ic
            mumps_par%JCN(nz)=jcih(ih)
C            MUMPS_PAR%JCN(NZ)=JDIH(IH)
          ENDDO
        END DO  
C
C     calcul de  b (r=b)
C
        DO ic=1,nc
          DO ik=iadll(ic),iadll(ic+1)-1
            i = indexlag(lll(ik))
            j = jll(ik)
            IF (j>3) THEN
              j = j-3
              r(ic) = r(ic) + xll(ik)*(vr(j,i)/dt12+ar(j,i))
            ELSE
              r(ic) = r(ic) + xll(ik)*(v(j,i)/dt12+a(j,i))
            ENDIF
          ENDDO
        ENDDO
        ALLOCATE(mumps_par%RHS(mumps_par%N))
        DO ic=1,nc
          mumps_par%RHS(ic) = r(ic)
        ENDDO
      END IF
C
C Resolution
C
      CALL spmd_mumps_exec(mumps_par,1)     ! analyse et factorisation
C
      CALL spmd_mumps_exec(mumps_par,2)     ! resolution  
C
C Resultat sur p0
C 
      IF(ispmd==0) THEN 
        DO ic=1,nc
          lambda(ic) = mumps_par%RHS(ic)
        ENDDO
      END IF
C
C Liberation de la memoire
C
      CALL spmd_mumps_deal(mumps_par)
CC  ---  Initialisation du vecteur solution
C      RNORM = ZERO
C      DO IC=1,NC
C        RNORM = RNORM + R(IC)*R(IC)
C        P(IC) = LAMBDA(IC)
C      ENDDO
CC---  Residu initial   R(i) = R(i) - sum(H(i,j)*P(j))
C      DO IC=1,NC
C        R(IC) = R(IC) - DIAG_H(IC)*P(IC)
C        DO IH=IADH(IC),IADH(IC+1)-1
C          JC = JCIH(IH)
C          R(IC) = R(IC) - HH(IH)*P(JC)
C          R(JC) = R(JC) - HH(IH)*P(IC)
C        ENDDO
C      ENDDO
C=======================================================================
C     iterations
C=======================================================================
C      ITERMAX = NC
C      ITER    = 0
C      DO WHILE(ITER<ITERMAX)
C        ITER  = ITER+1
C        NITER = ITER
CC--------------------
CC       preconditionnement  z = inv(H) r
CC--------------------
C        IF (LAGMOD==1) THEN
CC---    preconditionnement de Cholesky
C          CALL PRECHOL(Z,DIAG_L,HL,R,NC,IADH,JCIH)
C        ELSEIF(LAGMOD==2) THEN
CC---    preconditionnement polynomial :  H=I-B,  z = [I+B] r    
C          DO IC=1,NC
C            Z(IC) = R(IC) + R(IC)
C          ENDDO
C          DO IC=1,NC
C            Z(IC) = Z(IC) - DIAG_H(IC)*R(IC)
C            DO IH=IADH(IC),IADH(IC+1)-1
C              JC = JCIH(IH)
C              Z(IC) = Z(IC) - HH(IH)*R(JC)
C              Z(JC) = Z(JC) - HH(IH)*R(IC)
C            ENDDO
C          ENDDO
C        ENDIF
CC--------------------
C        R2    = R2NEW
C        R2NEW = ZERO
C        DO IC=1,NC
C          R2NEW = R2NEW + R(IC)*Z(IC)
C          Q(IC) = ZERO
C        ENDDO
CC--------------------
C        IF(ITER==1)THEN
C          DO IC=1,NC
C            P(IC) = Z(IC)
C          ENDDO
C        ELSE
C          BETA = R2NEW/R2
C          DO IC=1,NC
C            P(IC) = Z(IC) + BETA*P(IC)
C          ENDDO
C        ENDIF
CC-----------------------------------------------------------------------
CC     calcul de  q = q_ + [H] p
CC-----------------------------------------------------------------------
C      DO IC=1,NC
C        Q(IC) = Q(IC) + DIAG_H(IC)*P(IC)
C        DO IH=IADH(IC),IADH(IC+1)-1
C          JC = JCIH(IH)
C          Q(IC) = Q(IC) + HH(IH)*P(JC)          
C          Q(JC) = Q(JC) + HH(IH)*P(IC)          
C        ENDDO
C      ENDDO
CC-----------------------------------------------------------------------
CC     pt [H] p = pt q
CC-----------------------------------------------------------------------
C        PQ = ZERO
C        DO IC=1,NC
C          PQ = PQ + P(IC)*Q(IC)
C        ENDDO
CC-----------------------------------------------------------------------
C        IF(PQ==ZERO)THEN
C          ITER = ITERMAX
C        ELSE
C          ALPHA = R2NEW/PQ
C          LAG_ERSQ2 = ZERO
C          DO IC=1,NC
C            LAMBDA(IC) = LAMBDA(IC) + ALPHA*P(IC)
C            R(IC)      = R(IC)      - ALPHA*Q(IC)
C            LAG_ERSQ2  = LAG_ERSQ2  + R(IC)*R(IC)
C          ENDDO
CC--------- test convergence --------------------------------------------
C          LAG_ERSQ2 = LAG_ERSQ2/MAX(EM30,RNORM)
C          IF(LAG_ERSQ2<LAGM_TOL)THEN
C            ITER = ITERMAX
C          ENDIF
C        ENDIF
C=======================================================================
C     fin boucle gradient conjugue
C=======================================================================
C      ENDDO
C      NITER_GC = NITER
C---
C     a = ao + [M]-1[L]t la  
C---
      IF(ispmd==0) THEN 
       DO ic=1,ncr
        DO ik=iadll(ic),iadll(ic+1)-1
          i = indexlag(lll(ik))
          j = jll(ik)
          xll(ik) = xll(ik)*lambda(ic)
          IF(j>3) THEN
            j = j-3
            ar(j,i) = ar(j,i) - xll(ik)/in(i)
          ELSE         
            a(j,i)  = a(j,i)  - xll(ik)/mas(i)
          ENDIF         
        ENDDO
       ENDDO
      END IF
#else
        write(6,*) "Error: this feature requires the MUMPS library "
        call flush(6)
        CALL arret(5)
#endif
C--------------------------------------------
      RETURN
      END