fstr_dynamic_nlexplicit.f90

Go to the documentation of this file.

!-------------------------------------------------------------------------------
! Copyright (c) 2019 FrontISTR Commons
! This software is released under the MIT License, see LICENSE.txt
!-------------------------------------------------------------------------------
 
module fstr_dynamic_nlexplicit
  use m_fstr
  use m_static_lib
  use m_dynamic_output
  use m_fstr_eig_setmass
  use m_dynamic_mat_ass_bc_ac
  use m_dynamic_mat_ass_bc
  use m_dynamic_mat_ass_bc_vl
  use m_dynamic_mat_ass_load
  use m_fstr_update
  use m_fstr_residual
  use m_fstr_restart
  use m_dynamic_mat_ass_couple
  use m_fstr_rcap_io
  use mcontact
  use m_fstr_contact_elem_common, only: computetm_tt
  use m_fstr_timeinc
 
contains
 
  !C================================================================C
  !C-- subroutine  fstr_solve_LINEAR_DYNAMIC
  !C================================================================C
  subroutine fstr_solve_dynamic_nlexplicit(hecMESH,hecMAT,fstrSOLID,fstrEIG   &
      ,fstrDYN,fstrPARAM,infoCTChange &
      ,fstrCPL, restrt_step_num, restrt_step_count )
    implicit none
    type(hecmwst_local_mesh)             :: hecMESH
    type(hecmwst_matrix)                 :: hecMAT
    type(fstr_eigen)                     :: fstrEIG
    type(fstr_solid)                     :: fstrSOLID
    type(fstr_param)                     :: fstrPARAM
    type(fstr_dynamic)                   :: fstrDYN
    type(fstr_info_contactchange)        :: infoCTChange
    type(fstr_couple)                    :: fstrCPL !for COUPLE
    type(hecmwst_matrix), pointer :: hecMATmpc
    integer(kind=kint) :: nnod, ndof, nn
    integer(kind=kint) :: ierror
    real(kind=kreal), allocatable :: prevb(:)
    integer(kind=kint) :: restrt_step_num
    integer(kind=kint) :: restrt_step_count
    real(kind=kreal), parameter :: pi = 3.14159265358979323846d0
    integer(kind=kint) :: tot_step, sub_step, step_count
    logical :: is_OutPoint
 
    call hecmw_mpc_mat_init_explicit(hecmesh, hecmat, hecmatmpc)
 
    hecmat%NDOF=hecmesh%n_dof
    nnod=hecmesh%n_node
    ndof=hecmat%NDOF
    nn=ndof*ndof
 
    if( fstrparam%fg_couple == 1) then
      if( fstrparam%fg_couple_type==5 .or. &
          fstrparam%fg_couple_type==6 ) then
        allocate( prevb(hecmat%NP*ndof)      ,stat=ierror )
        prevb = 0.0d0
        if( ierror /= 0 ) then
          write(idbg,*) 'stop due to allocation error <fstr_solve_NONLINEAR_DYNAMIC, prevB>'
          write(idbg,*) '  rank = ', hecmesh%my_rank,'  ierror = ',ierror
          call flush(idbg)
          call hecmw_abort( hecmw_comm_get_comm())
        endif
      endif
    endif
 
    fstrsolid%dunode(:) =0.d0
 
    call fstr_prepare_dynamic_explicit( hecmesh, hecmat, hecmatmpc, fstrsolid, fstreig, fstrdyn, &
      & ndof, nnod, restrt_step_count )
 
    if( associated( fstrsolid%contacts ) )  then
      if( ndof /= 3 ) then
        if( hecmesh%my_rank == 0 ) write(*,*) 'Explicit contact analysis requires three displacement DOFs per node'
        call hecmw_abort(hecmw_comm_get_comm())
      endif
      call initialize_contact_output_vectors(fstrsolid,hecmat)
      call setup_contact_elesurf_for_area( hecmesh, fstrsolid )
      call forward_increment_lagrange(1,ndof,fstrdyn%VEC1,hecmesh,fstrsolid,infoctchange,&
        & fstrsolid%ddunode,restrt_step_count == 0)
    endif
 
    if( restrt_step_count == 0 ) then
      call fstr_dynamic_output(1, 0, 0.d0, hecmesh, fstrsolid, fstrdyn, fstrparam, .true.)
      call dynamic_output_monit(1, 0, 0.d0, hecmesh, fstrdyn, fstreig, fstrsolid)
    end if
 
    step_count = restrt_step_count
    do tot_step = 1, fstrsolid%nstep_tot
      if(hecmesh%my_rank==0) write(*,'(a,i5)') ' loading step=',tot_step
 
      sub_step = restrt_step_num
      do while(.true.)
        call fstr_timeinc_settimeincrement( fstrsolid%step_ctrl(tot_step), fstrparam, sub_step, fstrsolid )
 
        fstrdyn%t_curr = fstr_get_time()
        fstrdyn%t_delta = fstr_get_timeinc()
 
        step_count = step_count + 1
 
        call fstr_advance_dynamic_explicit( tot_step, sub_step, &
            hecmesh, hecmat, hecmatmpc, fstrsolid, fstreig, fstrdyn, fstrparam, &
            fstrcpl, infoctchange, &
            restrt_step_num, ndof, nnod, prevb, &
            fstr_timeinc_isstepfinished( fstrsolid%step_ctrl(tot_step) ) )
 
        ! ----- Result output (include visualize output)
        ! Evaluate isTimePoint before time advance, then OR with isStepFinished after
        is_outpoint = fstr_timeinc_istimepoint( fstrsolid%step_ctrl(tot_step), fstrparam )
 
          !C-- output result of monitoring node
        call dynamic_output_monit(tot_step, sub_step, fstrdyn%t_curr, hecmesh, fstrdyn, fstreig, fstrsolid)
 
        if( fstrdyn%restart_nout > 0 ) then
          if ( mod(step_count,fstrdyn%restart_nout).eq.0 ) then
            call fstr_write_restart_dyna_nl(tot_step,sub_step,hecmesh,fstrsolid,fstrdyn,fstrparam,&
              .false.,infoctchange%contactNode_current,step_count)
          end if
        end if
 
        call fstr_proceed_time()
        fstrdyn%t_curr = fstr_get_time()
 
        ! isStepFinished must be evaluated after fstr_proceed_time
        is_outpoint = is_outpoint .or. fstr_timeinc_isstepfinished( fstrsolid%step_ctrl(tot_step) )
 
          !C-- output new displacement, velocity and acceleration
        call fstr_dynamic_output(tot_step, step_count, fstrdyn%t_curr, hecmesh, fstrsolid, fstrdyn, fstrparam, is_outpoint)
 
        if( fstr_timeinc_isstepfinished( fstrsolid%step_ctrl(tot_step) ) ) exit
 
        if( sub_step == fstrsolid%step_ctrl(tot_step)%num_substep ) then
          if( hecmesh%my_rank == 0 ) then
            write(*,'(a,i5,a,f6.3)') '### Number of substeps reached max number: at total_step=', &
              & tot_step, '  time=', fstr_get_time()
          endif
          call fstr_abort( hecmw_exit_noconv )
        endif
 
        sub_step = sub_step + 1
      enddo
 
      if( fstrdyn%restart_nout > 0 ) then
        call fstr_write_restart_dyna_nl(tot_step,sub_step,hecmesh,fstrsolid,fstrdyn,fstrparam,&
          .true.,infoctchange%contactNode_current,step_count)
      end if
      restrt_step_num = 1
    enddo
 
    if( fstrparam%fg_couple == 1) then
      if( fstrparam%fg_couple_type==5 .or. &
          fstrparam%fg_couple_type==6 ) then
        deallocate( prevb      ,stat=ierror )
        if( ierror /= 0 ) then
          write(idbg,*) 'stop due to deallocation error <fstr_solve_NONLINEAR_DYNAMIC, prevB>'
          write(idbg,*) '  rank = ', hecmesh%my_rank,'  ierror = ',ierror
          call flush(idbg)
          call hecmw_abort( hecmw_comm_get_comm())
        endif
      endif
    endif
 
    call hecmw_mpc_mat_finalize_explicit(hecmesh, hecmat, hecmatmpc)
 
  end subroutine fstr_solve_dynamic_nlexplicit
 
  subroutine fstr_prepare_dynamic_explicit( hecMESH, hecMAT, hecMATmpc, fstrSOLID, fstrEIG, fstrDYN, &
      ndof, nnod, restrt_step_count )
    implicit none
    type(hecmwst_local_mesh), intent(inout) :: hecMESH
    type(hecmwst_matrix), intent(inout)     :: hecMAT
    type(hecmwst_matrix), pointer, intent(inout) :: hecMATmpc
    type(fstr_solid), intent(inout)         :: fstrSOLID
    type(fstr_eigen), intent(inout)         :: fstrEIG
    type(fstr_dynamic), intent(inout)       :: fstrDYN
    integer(kind=kint), intent(in)          :: ndof
    integer(kind=kint), intent(in)          :: nnod
    integer(kind=kint), intent(in)          :: restrt_step_count
 
    integer(kind=kint), allocatable :: mark(:)
    integer(kind=kint) :: j
    real(kind=kreal)   :: a1, a2
 
    a1 = 1.d0/fstrdyn%t_delta**2
    a2 = 1.d0/(2.d0*fstrdyn%t_delta)
 
    call setmass(fstrsolid,hecmesh,hecmat,fstreig)
    call hecmw_mpc_trans_mass(hecmesh, hecmat, hecmatmpc, fstreig%mass)
 
    allocate(mark(hecmat%NP * hecmat%NDOF))
    call hecmw_mpc_mark_slave(hecmesh, hecmat, mark)
 
    do j = 1 ,ndof*nnod
      fstrdyn%VEC1(j) = (a1 + a2 *fstrdyn%ray_m) * fstreig%mass(j)
      if(mark(j) == 1) fstrdyn%VEC1(j) = 1.d0
      if(dabs(fstrdyn%VEC1(j)) < 1.0e-20) then
        if( hecmesh%my_rank == 0 ) then
          write(*,*) 'stop due to fstrDYN%VEC(j) = 0 ,  j = ', j
          write(imsg,*) 'stop due to fstrDYN%VEC(j) = 0 ,  j = ', j
        end if
        call hecmw_abort( hecmw_comm_get_comm())
      endif
    end do
 
    deallocate(mark)
 
    !C-- virtual past displacements for central-difference startup
    if( restrt_step_count == 0 ) then
      do j = 1 ,ndof*nnod
        fstrdyn%DISP(j,3) = fstrdyn%DISP(j,1) - fstrdyn%VEL (j,1)/(2.d0*a2)  + fstrdyn%ACC (j,1)/ (2.d0*a1)
        fstrdyn%DISP(j,2) = fstrdyn%DISP(j,1) - fstrdyn%VEL (j,1)/ a2 + fstrdyn%ACC (j,1)/ (2.d0*a1) * 4.d0
      end do
    endif
 
  end subroutine fstr_prepare_dynamic_explicit
 
  subroutine fstr_advance_dynamic_explicit( cstep, istep, &
      hecMESH, hecMAT, hecMATmpc, fstrSOLID, fstrEIG, fstrDYN, fstrPARAM, &
      fstrCPL, infoCTChange, &
      restrt_step_num, ndof, nnod, prevB, &
      is_last_step )
    implicit none
    integer(kind=kint), intent(in)               :: cstep
    integer(kind=kint), intent(in)               :: istep
    type(hecmwst_local_mesh), intent(inout)      :: hecMESH
    type(hecmwst_matrix), intent(inout)          :: hecMAT
    type(hecmwst_matrix), pointer, intent(inout) :: hecMATmpc
    type(fstr_solid), intent(inout)              :: fstrSOLID
    type(fstr_eigen), intent(inout)              :: fstrEIG
    type(fstr_dynamic), intent(inout)            :: fstrDYN
    type(fstr_param), intent(inout)              :: fstrPARAM
    type(fstr_couple), intent(inout)             :: fstrCPL
    type(fstr_info_contactchange), intent(inout) :: infoCTChange
    integer(kind=kint), intent(in)               :: restrt_step_num
    integer(kind=kint), intent(in)               :: ndof
    integer(kind=kint), intent(in)               :: nnod
    real(kind=kreal), allocatable, intent(inout) :: prevb(:)
    logical, intent(in)                          :: is_last_step
 
    integer(kind=kint) :: j, kk, kkk0, kkk1
    integer(kind=kint) :: revocap_flag
    real(kind=kreal)   :: bsize
    real(kind=kreal), parameter :: pi = 3.14159265358979323846d0
    real(kind=kreal)   :: a1, a2
    real(kind=kreal)   :: b1, b2, b3, a3
 
    !C-- central-difference coefficients (depend only on dt)
    a1 = 1.d0/fstrdyn%t_delta**2
    a2 = 1.d0/(2.d0*fstrdyn%t_delta)
    !C-- coupling-only coefficients (kept zero for central difference)
    a3 = 0.d0
    b1 = 0.d0; b2 = 0.d0; b3 = 0.d0
 
    !C-- mechanical boundary condition
    call dynamic_mat_ass_load (cstep, fstrdyn%t_curr, hecmesh, hecmat, fstrsolid, fstrdyn, fstrparam)
    do j=1, hecmesh%n_node*  hecmesh%n_dof
      hecmat%B(j)=hecmat%B(j)-fstrsolid%QFORCE(j)
    end do
 
    !C ********************************************************************************
    !C for couple analysis
    if( fstrparam%fg_couple == 1 ) then
      if( fstrparam%fg_couple_type==5 .or. &
          fstrparam%fg_couple_type==6 ) then
        do j = 1, hecmat%NP * ndof
          prevb(j) = hecmat%B(j)
        enddo
      endif
    endif
    do
      if( fstrparam%fg_couple == 1 ) then
        if( fstrparam%fg_couple_type==1 .or. &
          fstrparam%fg_couple_type==3 .or. &
          fstrparam%fg_couple_type==5 ) call fstr_rcap_get( fstrcpl )
        if( fstrparam%fg_couple_first /= 0 ) then
          bsize = dfloat( istep ) / dfloat( fstrparam%fg_couple_first )
          if( bsize > 1.0 ) bsize = 1.0
          do kkk0 = 1, fstrcpl%coupled_node_n
            kkk1 = 3 * kkk0
            fstrcpl%trac(kkk1-2) = bsize * fstrcpl%trac(kkk1-2)
            fstrcpl%trac(kkk1-1) = bsize * fstrcpl%trac(kkk1-1)
            fstrcpl%trac(kkk1  ) = bsize * fstrcpl%trac(kkk1  )
          enddo
        endif
        if( fstrparam%fg_couple_window > 0 ) then
          j = istep - restrt_step_num + 1
          kk = fstrdyn%n_step - restrt_step_num + 1
          bsize = 0.5*(1.0-cos(2.0*pi*dfloat(j)/dfloat(kk)))
          do kkk0 = 1, fstrcpl%coupled_node_n
            kkk1 = 3 * kkk0
            fstrcpl%trac(kkk1-2) = bsize * fstrcpl%trac(kkk1-2)
            fstrcpl%trac(kkk1-1) = bsize * fstrcpl%trac(kkk1-1)
            fstrcpl%trac(kkk1  ) = bsize * fstrcpl%trac(kkk1  )
          enddo
        endif
        call dynamic_mat_ass_couple( hecmesh, hecmat, fstrsolid, fstrcpl )
      endif
      !C ********************************************************************************
 
      call hecmw_mpc_trans_rhs(hecmesh, hecmat, hecmatmpc)
 
      do j = 1 ,ndof*nnod
        hecmatmpc%B(j) = hecmatmpc%B(j) + 2.d0*a1* fstreig%mass(j) * fstrdyn%DISP(j,1)  &
          + (- a1 + a2 * fstrdyn%ray_m) * fstreig%mass(j) * fstrdyn%DISP(j,3)
      end do
 
      !C
      !C-- geometrical boundary condition
 
      call dynamic_explicit_ass_bc(hecmesh, hecmatmpc, fstrsolid, fstrdyn, fstrdyn%t_curr)
      call dynamic_explicit_ass_vl(hecmesh, hecmatmpc, fstrsolid, fstrdyn, fstrdyn%t_curr)
      call dynamic_explicit_ass_ac(hecmesh, hecmatmpc, fstrsolid, fstrdyn, fstrdyn%t_curr)
 
      ! Finish the calculation
      do j = 1 ,ndof*nnod
        hecmatmpc%X(j) = hecmatmpc%B(j) / fstrdyn%VEC1(j)
        if(dabs(hecmatmpc%X(j)) > 1.0d+5) then
          if( hecmesh%my_rank == 0 ) then
            print *, 'Displacement increment too large, please adjust your step size!',istep,hecmatmpc%X(j)
            write(imsg,*) 'Displacement increment too large, please adjust your step size!',istep,hecmatmpc%B(j),fstrdyn%VEC1(j)
          end if
          call fstr_abort( hecmw_exit_noconv )
        end if
      end do
      call hecmw_mpc_tback_sol(hecmesh, hecmat, hecmatmpc)
 
      !C *****************************************************
      !C for couple analysis
      if( fstrparam%fg_couple == 1 ) then
        if( fstrparam%fg_couple_type>1 ) then
          do j=1, fstrcpl%coupled_node_n
            if( fstrcpl%dof == 3 ) then
              kkk0 = j*3
              kkk1 = fstrcpl%coupled_node(j)*3
 
              fstrcpl%disp (kkk0-2) = hecmat%X(kkk1-2)
              fstrcpl%disp (kkk0-1) = hecmat%X(kkk1-1)
              fstrcpl%disp (kkk0  ) = hecmat%X(kkk1  )
 
              fstrcpl%velo (kkk0-2) = -b1*fstrdyn%ACC(kkk1-2,1) - b2*fstrdyn%VEL(kkk1-2,1) + &
                b3*( hecmat%X(kkk1-2) - fstrdyn%DISP(kkk1-2,1) )
              fstrcpl%velo (kkk0-1) = -b1*fstrdyn%ACC(kkk1-1,1) - b2*fstrdyn%VEL(kkk1-1,1) + &
                b3*( hecmat%X(kkk1-1) - fstrdyn%DISP(kkk1-1,1) )
              fstrcpl%velo (kkk0  ) = -b1*fstrdyn%ACC(kkk1,1) - b2*fstrdyn%VEL(kkk1,1) + &
                b3*( hecmat%X(kkk1) - fstrdyn%DISP(kkk1,1) )
              fstrcpl%accel(kkk0-2) = -a1*fstrdyn%ACC(kkk1-2,1) - a2*fstrdyn%VEL(kkk1-2,1) + &
                a3*( hecmat%X(kkk1-2) - fstrdyn%DISP(kkk1-2,1) )
              fstrcpl%accel(kkk0-1) = -a1*fstrdyn%ACC(kkk1-1,1) - a2*fstrdyn%VEL(kkk1-1,1) + &
                a3*( hecmat%X(kkk1-1) - fstrdyn%DISP(kkk1-1,1) )
              fstrcpl%accel(kkk0  ) = -a1*fstrdyn%ACC(kkk1,1) - a2*fstrdyn%VEL(kkk1,1) + &
                a3*( hecmat%X(kkk1) - fstrdyn%DISP(kkk1,1) )
            else
              kkk0 = j*2
              kkk1 = fstrcpl%coupled_node(j)*2
 
              fstrcpl%disp (kkk0-1) = hecmat%X(kkk1-1)
              fstrcpl%disp (kkk0  ) = hecmat%X(kkk1  )
 
              fstrcpl%velo (kkk0-1) = -b1*fstrdyn%ACC(kkk1-1,1) - b2*fstrdyn%VEL(kkk1-1,1) + &
                b3*( hecmat%X(kkk1-1) - fstrdyn%DISP(kkk1-1,1) )
              fstrcpl%velo (kkk0  ) = -b1*fstrdyn%ACC(kkk1,1) - b2*fstrdyn%VEL(kkk1,1) + &
                b3*( hecmat%X(kkk1) - fstrdyn%DISP(kkk1,1) )
              fstrcpl%accel(kkk0-1) = -a1*fstrdyn%ACC(kkk1-1,1) - a2*fstrdyn%VEL(kkk1-1,1) + &
                a3*( hecmat%X(kkk1-1) - fstrdyn%DISP(kkk1-1,1) )
              fstrcpl%accel(kkk0  ) = -a1*fstrdyn%ACC(kkk1,1) - a2*fstrdyn%VEL(kkk1,1) + &
                a3*( hecmat%X(kkk1) - fstrdyn%DISP(kkk1,1) )
            endif
          end do
          call fstr_rcap_send( fstrcpl )
        endif
 
        select case ( fstrparam%fg_couple_type )
          case (4)
            call fstr_rcap_get( fstrcpl )
          case (5)
            call fstr_get_convergence( revocap_flag )
            if( revocap_flag==0 ) then
              do j = 1, hecmat%NP * ndof
                hecmat%B(j) = prevb(j)
              enddo
              cycle
            endif
          case (6)
            call fstr_get_convergence( revocap_flag )
            if( revocap_flag==0 ) then
              do j = 1, hecmat%NP * ndof
                hecmat%B(j) = prevb(j)
              enddo
              call fstr_rcap_get( fstrcpl )
              cycle
            else
              if( .not. is_last_step ) call fstr_rcap_get( fstrcpl )
            endif
        end select
      endif
      exit
    enddo
 
    !C *****************************************************
    !C-- contact corrector
    !C
    do j = 1 ,ndof*nnod
      fstrsolid%unode(j)  = fstrdyn%DISP(j,1)
      fstrsolid%dunode(j)  = hecmat%X(j)-fstrdyn%DISP(j,1)
    enddo
    if( associated( fstrsolid%contacts ) )  then
      !call fstr_scan_contact_state( cstep, fstrDYN%t_delta, kcaSLAGRANGE, hecMESH, fstrSOLID, infoCTChange )
      call forward_increment_lagrange(cstep,ndof,fstrdyn%VEC1,hecmesh,fstrsolid,infoctchange,&
        & fstrsolid%ddunode,.false.)
      do j = 1 ,ndof*nnod
        hecmat%X(j)  = hecmat%X(j) + fstrsolid%ddunode(j)
      enddo
    endif
 
    !C-- new displacement, velocity and acceleration
    do j = 1 ,ndof*nnod
      fstrdyn%ACC (j,1) = a1*(hecmat%X(j) - 2.d0*fstrdyn%DISP(j,1) + fstrdyn%DISP(j,3))
      fstrdyn%VEL (j,1) = a2*(hecmat%X(j) - fstrdyn%DISP(j,3))
      fstrsolid%unode(j)  = fstrdyn%DISP(j,1)
      fstrsolid%dunode(j)  = hecmat%X(j)-fstrdyn%DISP(j,1)
      fstrdyn%DISP(j,3) = fstrdyn%DISP(j,1)
      fstrdyn%DISP(j,1) = hecmat%X(j)
      hecmat%X(j)  = fstrsolid%dunode(j)
      fstrdyn%kineticEnergy = fstrdyn%kineticEnergy + 0.5d0*fstreig%mass(j)*fstrdyn%VEL(j,1)*fstrdyn%VEL(j,1)
    end do
 
    ! ----- update strain, stress, and internal force
    call fstr_updatenewton( hecmesh, hecmat, fstrsolid, fstrparam, fstrdyn%t_curr, fstrdyn%t_delta, 0, fstrdyn%strainEnergy )
 
    ! ----- update reaction force at constrained DOFs using converged QFORCE
    call fstr_update_reaction_spc( cstep, hecmesh, fstrsolid )
 
    do j = 1 ,ndof*nnod
      fstrsolid%unode(j) = fstrsolid%unode(j) + fstrsolid%dunode(j)
    end do
    call fstr_updatestate( hecmesh, fstrsolid, fstrdyn%t_delta )
 
  end subroutine fstr_advance_dynamic_explicit
 
  
  subroutine forward_increment_lagrange(cstep,ndof,mmat,hecMESH,fstrSOLID,infoCTChange,uc,is_init)
    integer, intent(in)                    :: cstep
    integer, intent(in)                    :: ndof
    real(kind=kreal), intent(in)           :: mmat(:)
    type( hecmwst_local_mesh ), intent(in) :: hecmesh
    type(fstr_solid), intent(inout)        :: fstrSOLID
    type(fstr_info_contactchange)          :: infoCTChange
    real(kind=kreal), intent(out)          :: uc(:)
    logical, intent(in)                    :: is_init
    integer :: i, j, k, slave, nn, iSS, sid, iter, offset
    real(kind=kreal), allocatable :: contact_force(:), friction_force(:), compliance(:)
    real(kind=kreal) :: fdum, conv, dlambda, rel_t(3), tangent(3), friction(3)
    real(kind=kreal) :: tangent_compliance, friction_limit, friction_norm, rel_t_norm
    real(kind=kreal) :: element_disp(3*(l_max_surface_node+1))
    real(kind=kreal) :: tm(3,3*(l_max_surface_node+1)), tt(3,3*(l_max_surface_node+1))
    real(kind=kreal) :: bn(3*(l_max_surface_node+1))
    integer, parameter :: MAX_CONTACT_ITER = 1000
 
    uc = 0.d0
    call fstr_scan_contact_state_exp( cstep, is_init, hecmesh, fstrsolid, infoctchange )
    if( .not. infoctchange%active ) return
 
    allocate(contact_force(hecmesh%n_node*ndof), friction_force(hecmesh%n_node*ndof), &
      compliance(hecmesh%n_node*ndof))
    compliance = 1.d0/mmat
    call mask_explicit_contact_constraints(ndof, hecmesh, fstrsolid, compliance)
 
    do i=1,fstrsolid%n_contacts
      do j=1,size(fstrsolid%contacts(i)%slave)
        if( .not. is_contact_active(fstrsolid%contacts(i)%states(j)%state) ) cycle
        fstrsolid%contacts(i)%states(j)%multiplier(:) = 0.d0
        fstrsolid%contacts(i)%states(j)%wkdist = 0.d0
      enddo
    enddo
 
    iter = 0
    do
      contact_force = 0.d0
      do i=1,fstrsolid%n_contacts
        do j= 1, size(fstrsolid%contacts(i)%slave)
          if( .not. is_contact_active(fstrsolid%contacts(i)%states(j)%state) ) cycle
          if( fstrsolid%contacts(i)%states(j)%distance>epsilon(1.d0) ) then
            fstrsolid%contacts(i)%states(j)%state = contactfree
            cycle
          endif
          slave = fstrsolid%contacts(i)%slave(j)
          sid = fstrsolid%contacts(i)%states(j)%surface
          nn = size( fstrsolid%contacts(i)%master(sid)%nodes )
          if( nn > l_max_surface_node ) then
            if( hecmesh%my_rank == 0 ) write(*,*) 'Too many nodes on explicit contact surface:', nn
            call hecmw_abort(hecmw_comm_get_comm())
          endif
          call computetm_tt(fstrsolid%contacts(i)%states(j), fstrsolid%contacts(i)%master(sid), &
            0.d0, tm, tt, fstrsolid%contacts(i)%smoothing, bn)
          offset = (slave-1)*ndof
          contact_force(offset+1:offset+3) = contact_force(offset+1:offset+3) &
            + fstrsolid%contacts(i)%states(j)%multiplier(1)*bn(1:3)
          do k=1,nn
            iss = fstrsolid%contacts(i)%master(sid)%nodes(k)
            offset = (iss-1)*ndof
            contact_force(offset+1:offset+3) = contact_force(offset+1:offset+3) &
              + fstrsolid%contacts(i)%states(j)%multiplier(1)*bn(3*k+1:3*k+3)
          enddo
        enddo
      enddo
 
      call hecmw_assemble_r(hecmesh, contact_force, hecmesh%n_node, ndof)
      call hecmw_update_r(hecmesh, contact_force, hecmesh%n_node, ndof)
 
      conv = 0.d0
      do i=1,fstrsolid%n_contacts
        do j= 1, size(fstrsolid%contacts(i)%slave)
          if( .not. is_contact_active(fstrsolid%contacts(i)%states(j)%state) ) cycle
          slave = fstrsolid%contacts(i)%slave(j)
          sid = fstrsolid%contacts(i)%states(j)%surface
          nn = size( fstrsolid%contacts(i)%master(sid)%nodes )
          call computetm_tt(fstrsolid%contacts(i)%states(j), fstrsolid%contacts(i)%master(sid), &
            0.d0, tm, tt, fstrsolid%contacts(i)%smoothing, bn)
          offset = (slave-1)*ndof
          fstrsolid%contacts(i)%states(j)%wkdist = &
            dot_product(bn(1:3), contact_force(offset+1:offset+3)*compliance(offset+1:offset+3))
          fdum = dot_product(bn(1:3), bn(1:3)*compliance(offset+1:offset+3))
          do k=1,nn
            iss = fstrsolid%contacts(i)%master(sid)%nodes(k)
            offset = (iss-1)*ndof
            fstrsolid%contacts(i)%states(j)%wkdist = fstrsolid%contacts(i)%states(j)%wkdist &
              + dot_product(bn(3*k+1:3*k+3), contact_force(offset+1:offset+3)*compliance(offset+1:offset+3))
            fdum = fdum + dot_product(bn(3*k+1:3*k+3), bn(3*k+1:3*k+3)*compliance(offset+1:offset+3))
          enddo
          dlambda = 0.d0
          if( fdum > 1.d-20 ) &
            dlambda = (fstrsolid%contacts(i)%states(j)%distance-fstrsolid%contacts(i)%states(j)%wkdist)/fdum
          conv = conv + dlambda*dlambda
          fstrsolid%contacts(i)%states(j)%multiplier(1) = fstrsolid%contacts(i)%states(j)%multiplier(1) + dlambda
        enddo
      enddo
      call hecmw_allreduce_r1(hecmesh, conv, hecmw_sum)
      iter = iter+1
      if( dsqrt(conv)<1.d-8 ) exit
      if( iter >= max_contact_iter ) then
        if( hecmesh%my_rank == 0 ) write(*,*) 'Explicit contact correction failed to converge'
        call hecmw_abort(hecmw_comm_get_comm())
      endif
    enddo
 
    contact_force = 0.d0
    do i=1,fstrsolid%n_contacts
      do j=1,size(fstrsolid%contacts(i)%slave)
        if( .not. is_contact_active(fstrsolid%contacts(i)%states(j)%state) ) cycle
        slave = fstrsolid%contacts(i)%slave(j)
        sid = fstrsolid%contacts(i)%states(j)%surface
        nn = size(fstrsolid%contacts(i)%master(sid)%nodes)
        call computetm_tt(fstrsolid%contacts(i)%states(j), fstrsolid%contacts(i)%master(sid), &
          0.d0, tm, tt, fstrsolid%contacts(i)%smoothing, bn)
        offset = (slave-1)*ndof
        contact_force(offset+1:offset+3) = contact_force(offset+1:offset+3) &
          + fstrsolid%contacts(i)%states(j)%multiplier(1)*bn(1:3)
        do k=1,nn
          iss = fstrsolid%contacts(i)%master(sid)%nodes(k)
          offset = (iss-1)*ndof
          contact_force(offset+1:offset+3) = contact_force(offset+1:offset+3) &
            + fstrsolid%contacts(i)%states(j)%multiplier(1)*bn(3*k+1:3*k+3)
        enddo
      enddo
    enddo
    call hecmw_assemble_r(hecmesh, contact_force, hecmesh%n_node, ndof)
    call hecmw_update_r(hecmesh, contact_force, hecmesh%n_node, ndof)
 
    do i=1,hecmesh%n_node*ndof
      uc(i) = contact_force(i)*compliance(i)
    enddo
 
    friction_force = 0.d0
    do i=1,fstrsolid%n_contacts
      do j=1,size(fstrsolid%contacts(i)%slave)
        if( .not. is_contact_active(fstrsolid%contacts(i)%states(j)%state) ) cycle
        if( fstrsolid%contacts(i)%fcoeff <= 0.d0 ) then
          fstrsolid%contacts(i)%states(j)%tangentForce_final(:) = 0.d0
          fstrsolid%contacts(i)%states(j)%reldisp(:) = 0.d0
          cycle
        endif
 
        slave = fstrsolid%contacts(i)%slave(j)
        sid = fstrsolid%contacts(i)%states(j)%surface
        nn = size(fstrsolid%contacts(i)%master(sid)%nodes)
        call computetm_tt(fstrsolid%contacts(i)%states(j), fstrsolid%contacts(i)%master(sid), &
          fstrsolid%contacts(i)%fcoeff, tm, tt, fstrsolid%contacts(i)%smoothing)
 
        offset = (slave-1)*ndof
        element_disp(1:3) = fstrsolid%dunode(offset+1:offset+3) + uc(offset+1:offset+3)
        do k=1,nn
          iss = fstrsolid%contacts(i)%master(sid)%nodes(k)
          offset = (iss-1)*ndof
          element_disp(3*k+1:3*k+3) = fstrsolid%dunode(offset+1:offset+3) + uc(offset+1:offset+3)
        enddo
        rel_t = matmul(tt(1:3,1:3*(nn+1)), element_disp(1:3*(nn+1)))
        fstrsolid%contacts(i)%states(j)%reldisp(:) = rel_t
 
        rel_t_norm = dsqrt(dot_product(rel_t,rel_t))
        friction = 0.d0
        if( rel_t_norm > 1.d-20 ) then
          tangent = rel_t/rel_t_norm
          offset = (slave-1)*ndof
          tangent_compliance = sum(matmul(tangent,tt(1:3,1:3))**2*compliance(offset+1:offset+3))
          do k=1,nn
            iss = fstrsolid%contacts(i)%master(sid)%nodes(k)
            offset = (iss-1)*ndof
            tangent_compliance = tangent_compliance &
              + sum(matmul(tangent,tt(1:3,3*k+1:3*k+3))**2*compliance(offset+1:offset+3))
          enddo
          if( tangent_compliance > 1.d-20 ) friction = rel_t/tangent_compliance
        endif
 
        friction_limit = fstrsolid%contacts(i)%fcoeff &
          * dabs(fstrsolid%contacts(i)%states(j)%multiplier(1))
        friction_norm = dsqrt(dot_product(friction,friction))
        if( friction_norm > friction_limit .and. friction_norm > 1.d-20 ) then
          friction = friction*(friction_limit/friction_norm)
          fstrsolid%contacts(i)%states(j)%state = contactslip
        else
          fstrsolid%contacts(i)%states(j)%state = contactstick
        endif
        friction_norm = dsqrt(dot_product(friction,friction))
        fstrsolid%contacts(i)%states(j)%tangentForce_final(:) = friction
        fstrsolid%contacts(i)%states(j)%multiplier(2) = friction_norm
        fstrsolid%contacts(i)%states(j)%multiplier(3) = 0.d0
 
        offset = (slave-1)*ndof
        friction_force(offset+1:offset+3) = friction_force(offset+1:offset+3) &
          - matmul(transpose(tm(1:3,1:3)), friction)
        do k=1,nn
          iss = fstrsolid%contacts(i)%master(sid)%nodes(k)
          offset = (iss-1)*ndof
          friction_force(offset+1:offset+3) = friction_force(offset+1:offset+3) &
            - matmul(transpose(tm(1:3,3*k+1:3*k+3)), friction)
        enddo
      enddo
    enddo
    call hecmw_assemble_r(hecmesh, friction_force, hecmesh%n_node, ndof)
    call hecmw_update_r(hecmesh, friction_force, hecmesh%n_node, ndof)
    do i=1,hecmesh%n_node*ndof
      uc(i) = (contact_force(i)+friction_force(i))*compliance(i)
    enddo
 
    call fstr_update_contact_tangentforce(cstep, fstrsolid)
    deallocate(contact_force, friction_force, compliance)
  end subroutine forward_increment_lagrange
 
  subroutine mask_explicit_contact_constraints(ndof, hecMESH, fstrSOLID, compliance)
    integer(kind=kint), intent(in)          :: ndof
    type(hecmwst_local_mesh), intent(in)    :: hecMESH
    type(fstr_solid), intent(in)            :: fstrSOLID
    real(kind=kreal), intent(inout)         :: compliance(:)
    integer(kind=kint) :: ig0, ig, ityp, idofS, idofE, iS0, iE0, ik, node, idof
 
    do ig0=1,fstrsolid%BOUNDARY_ngrp_tot
      ig = fstrsolid%BOUNDARY_ngrp_ID(ig0)
      ityp = fstrsolid%BOUNDARY_ngrp_type(ig0)
      idofs = ityp/10
      idofe = ityp-idofs*10
      if( fstrsolid%BOUNDARY_ngrp_rotID(ig0) > 0 ) then
        idofs = 1
        idofe = ndof
      endif
      is0 = hecmesh%node_group%grp_index(ig-1)+1
      ie0 = hecmesh%node_group%grp_index(ig)
      do ik=is0,ie0
        node = hecmesh%node_group%grp_item(ik)
        do idof=idofs,min(idofe,ndof)
          compliance((node-1)*ndof+idof) = 0.d0
        enddo
      enddo
    enddo
 
    if( fstrsolid%VELOCITY_type /= kbcinitial ) then
      do ig0=1,fstrsolid%VELOCITY_ngrp_tot
        ig = fstrsolid%VELOCITY_ngrp_ID(ig0)
        ityp = fstrsolid%VELOCITY_ngrp_type(ig0)
        idofs = ityp/10
        idofe = ityp-idofs*10
        if( fstrsolid%VELOCITY_ngrp_rotID(ig0) > 0 ) then
          idofs = 1
          idofe = ndof
        endif
        is0 = hecmesh%node_group%grp_index(ig-1)+1
        ie0 = hecmesh%node_group%grp_index(ig)
        do ik=is0,ie0
          node = hecmesh%node_group%grp_item(ik)
          do idof=idofs,min(idofe,ndof)
            compliance((node-1)*ndof+idof) = 0.d0
          enddo
        enddo
      enddo
    endif
 
    if( fstrsolid%ACCELERATION_type /= kbcinitial ) then
      do ig0=1,fstrsolid%ACCELERATION_ngrp_tot
        ig = fstrsolid%ACCELERATION_ngrp_ID(ig0)
        ityp = fstrsolid%ACCELERATION_ngrp_type(ig0)
        idofs = ityp/10
        idofe = ityp-idofs*10
        is0 = hecmesh%node_group%grp_index(ig-1)+1
        ie0 = hecmesh%node_group%grp_index(ig)
        do ik=is0,ie0
          node = hecmesh%node_group%grp_item(ik)
          do idof=idofs,min(idofe,ndof)
            compliance((node-1)*ndof+idof) = 0.d0
          enddo
        enddo
      enddo
    endif
  end subroutine mask_explicit_contact_constraints
 
end module fstr_dynamic_nlexplicit