fstr_dynamic_nlimplicit.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_nlimplicit
  use m_fstr
  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_stiffmatrix
  use m_fstr_update
  use m_fstr_restart
  use fstr_matrix_con_contact
  use m_fstr_residual
  use mcontact
  use m_addcontactstiffness
  use m_solve_lineq_contact
  use m_dynamic_init_variables
 
  !-------- for couple -------
  use m_dynamic_mat_ass_couple
  use m_fstr_rcap_io
 
contains
 
 
  subroutine fstr_solve_dynamic_nlimplicit(cstep, hecMESH,hecMAT,fstrSOLID,fstrEIG, &
      fstrDYNAMIC,fstrRESULT,fstrPARAM,fstrCPL, restrt_step_num )
    implicit none
    !C-- global variable
    integer, intent(in)                  :: cstep
    type(hecmwst_local_mesh)             :: hecMESH
    type(hecmwst_matrix)                 :: hecMAT
    type(fstr_eigen)                     :: fstrEIG
    type(fstr_solid)                     :: fstrSOLID
    type(hecmwst_result_data)            :: fstrRESULT
    type(fstr_param)                     :: fstrPARAM
    type(fstr_dynamic)                   :: fstrDYNAMIC
    type(hecmwst_matrix_lagrange)        :: hecLagMAT
    type(fstr_couple)                    :: fstrCPL !for COUPLE
 
    !C-- local variable
    type(hecmwst_local_mesh), pointer :: hecMESHmpc
    type(hecmwst_matrix), pointer :: hecMATmpc
    type(hecmwst_matrix), pointer :: hecMAT0
    integer(kind=kint) :: nnod, ndof, numnp, nn
    integer(kind=kint) :: i, j, ids, ide, ims, ime, kk, idm, imm
    integer(kind=kint) :: iter
    integer(kind=kint) :: iiii5, iexit
    integer(kind=kint) :: revocap_flag
    integer(kind=kint) :: kkk0, kkk1
    integer(kind=kint) :: restrt_step_num
    integer(kind=kint) :: n_node_global
    integer(kind=kint) :: ierr
 
    real(kind=kreal) :: a1, a2, a3, b1, b2, b3, c1, c2
    real(kind=kreal) :: bsize, res, resb
    real(kind=kreal) :: time_1, time_2
    real(kind=kreal), parameter :: pi = 3.14159265358979323846d0
    real(kind=kreal), allocatable :: coord(:)
 
    logical :: is_cycle
 
    iexit = 0
    resb = 0.0d0
 
    call hecmw_mpc_mat_init(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
    nullify(hecmat0)
 
    ! sum of n_node among all subdomains (to be used to calc res)
    n_node_global = hecmesh%nn_internal
    call hecmw_allreduce_i1(hecmesh,n_node_global,hecmw_sum)
 
    hecmat%NDOF=hecmesh%n_dof
    nnod=hecmesh%n_node
    ndof=hecmat%NDOF
    nn  =ndof*ndof
 
    allocate(coord(hecmesh%n_node*ndof))
 
    !!-- initial value
    time_1 = hecmw_wtime()
 
    !C-- check parameters
    if(dabs(fstrdynamic%beta) < 1.0e-20) then
      if( hecmesh%my_rank == 0 ) then
        write(imsg,*) 'stop due to Newmark-beta = 0'
      endif
      call hecmw_abort( hecmw_comm_get_comm())
    endif
 
    !C-- matrix [M] lumped mass matrix
    if(fstrdynamic%idx_mas == 1) then
      call setmass(fstrsolid,hecmesh,hecmat,fstreig)
 
    !C-- consistent mass matrix
    else if(fstrdynamic%idx_mas == 2) then
      if( hecmesh%my_rank .eq. 0 ) then
        write(imsg,*) 'stop: consistent mass matrix is not yet available !'
      endif
      call hecmw_abort( hecmw_comm_get_comm())
    endif
 
    hecmat%Iarray(98) = 1   !Assembly complete
    hecmat%Iarray(97) = 1   !Need numerical factorization
 
    !C-- time step loop
    a1 = 0.5d0/fstrdynamic%beta - 1.0d0
    a2 = 1.0d0/(fstrdynamic%beta*fstrdynamic%t_delta)
    a3 = 1.0d0/(fstrdynamic%beta*fstrdynamic%t_delta*fstrdynamic%t_delta)
    b1 = (0.5d0*fstrdynamic%gamma/fstrdynamic%beta - 1.0d0 )*fstrdynamic%t_delta
    b2 = fstrdynamic%gamma/fstrdynamic%beta - 1.0d0
    b3 = fstrdynamic%gamma/(fstrdynamic%beta*fstrdynamic%t_delta)
    c1 = 1.0d0 + fstrdynamic%ray_k*b3
    c2 = a3 + fstrdynamic%ray_m*b3
 
    !C-- output of initial state
    if( restrt_step_num == 1 ) then
      call fstr_dynamic_output(hecmesh, fstrsolid, fstrdynamic, fstrparam)
      call dynamic_output_monit(hecmesh, fstrparam, fstrdynamic, fstreig, fstrsolid)
    endif
 
    fstrdynamic%VEC3(:) =0.0d0
    hecmat%X(:) =0.0d0
 
    !! step = 1,2,....,fstrDYNAMIC%n_step
    do i = restrt_step_num, fstrdynamic%n_step
      if(ndof == 4 .and. hecmesh%my_rank==0) write(*,'(a,i5)')"iter: ",i
 
      fstrdynamic%i_step = i
      fstrdynamic%t_curr = fstrdynamic%t_delta * i
 
      if(hecmesh%my_rank==0) then
        !write(*,'('' time step='',i10,'' time='',1pe13.4e3)') i,fstrDYNAMIC%t_curr
        write(ista,'('' time step='',i10,'' time='',1pe13.4e3)') i,fstrdynamic%t_curr
      endif
 
      do j = 1 ,ndof*nnod
        fstrdynamic%VEC1(j) = a1*fstrdynamic%ACC(j,1) + a2*fstrdynamic%VEL(j,1)
        fstrdynamic%VEC2(j) = b1*fstrdynamic%ACC(j,1) + b2*fstrdynamic%VEL(j,1)
      enddo
 
      !C ********************************************************************************
      !C for couple analysis
      do
        fstrsolid%dunode(:) =0.d0
        ! call fstr_UpdateEPState( hecMESH, fstrSOLID )
        call fstr_solve_dynamic_nlimplicit_couple_init(fstrparam, fstrcpl)
 
        do iter = 1, fstrsolid%step_ctrl(cstep)%max_iter
          if (fstrparam%nlgeom) then
            call fstr_stiffmatrix( hecmesh, hecmat, fstrsolid, fstrdynamic%t_curr, fstrdynamic%t_delta )
          else
            if (.not. associated(hecmat0)) then
              call fstr_stiffmatrix( hecmesh, hecmat, fstrsolid, fstrdynamic%t_curr, fstrdynamic%t_delta )
              allocate(hecmat0)
              call hecmw_mat_init(hecmat0)
              call hecmw_mat_copy_profile(hecmat, hecmat0)
              call hecmw_mat_copy_val(hecmat, hecmat0)
            else
              call hecmw_mat_copy_val(hecmat0, hecmat)
            endif
          endif
 
          if( fstrdynamic%ray_k/=0.d0 .or. fstrdynamic%ray_m/=0.d0 ) then
            do j = 1 ,ndof*nnod
              hecmat%X(j) = fstrdynamic%VEC2(j) - b3*fstrsolid%dunode(j)
            enddo
          endif
          if( fstrdynamic%ray_k/=0.d0 ) then
            if( hecmesh%n_dof == 3 ) then
              call hecmw_matvec (hecmesh, hecmat, hecmat%X, fstrdynamic%VEC3)
            else if( hecmesh%n_dof == 2 ) then
              call hecmw_matvec (hecmesh, hecmat, hecmat%X, fstrdynamic%VEC3)
            else if( hecmesh%n_dof == 6 ) then
              call matvec(fstrdynamic%VEC3, hecmat%X, hecmat, ndof, hecmat%D, hecmat%AU, hecmat%AL)
            endif
          endif
 
          !C-- mechanical boundary condition
          call dynamic_mat_ass_load (hecmesh, hecmat, fstrsolid, fstrdynamic, fstrparam, iter)
          do j=1, hecmesh%n_node*  hecmesh%n_dof
            hecmat%B(j) = hecmat%B(j)- fstrsolid%QFORCE(j) + fstreig%mass(j)*( fstrdynamic%VEC1(j)-a3*fstrsolid%dunode(j) &
              + fstrdynamic%ray_m* hecmat%X(j) ) + fstrdynamic%ray_k*fstrdynamic%VEC3(j)
          enddo
 
          !C ********************************************************************************
          !C for couple analysis
          call fstr_solve_dynamic_nlimplicit_couple_pre(hecmesh, hecmat, fstrsolid, &
            & fstrparam, fstrdynamic, fstrcpl, restrt_step_num, pi, i)
 
          do j = 1 ,nn*hecmat%NP
            hecmat%D(j)  = c1* hecmat%D(j)
          enddo
          do j = 1 ,nn*hecmat%NPU
            hecmat%AU(j) = c1* hecmat%AU(j)
          enddo
          do j = 1 ,nn*hecmat%NPL
            hecmat%AL(j) = c1*hecmat%AL(j)
          enddo
          do j=1,nnod
            do kk=1,ndof
              idm = nn*(j-1)+1 + (ndof+1)*(kk-1)
              imm = ndof*(j-1) + kk
              hecmat%D(idm) = hecmat%D(idm) + c2*fstreig%mass(imm)
            enddo
          enddo
 
          !C-- geometrical boundary condition
          call dynamic_mat_ass_bc   (hecmesh, hecmat, fstrsolid, fstrdynamic, fstrparam, heclagmat, iter)
          call dynamic_mat_ass_bc_vl(hecmesh, hecmat, fstrsolid, fstrdynamic, fstrparam, heclagmat, iter)
          call dynamic_mat_ass_bc_ac(hecmesh, hecmat, fstrsolid, fstrdynamic, fstrparam, heclagmat, iter)
          call hecmw_mpc_mat_ass(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
          call hecmw_mpc_trans_rhs(hecmesh, hecmat, hecmatmpc)
 
          !C-- RHS LOAD VECTOR CHECK
          numnp=hecmatmpc%NP
          call hecmw_innerproduct_r(hecmesh, ndof, hecmatmpc%B, hecmatmpc%B, bsize)
 
          if(iter == 1)then
            resb = bsize
          endif
 
          !res = dsqrt(bsize)/n_node_global
          res = dsqrt(bsize/resb)
          if( fstrparam%nlgeom .and. ndof /= 4 ) then
            if(hecmesh%my_rank==0) write(*,'(a,i5,a,1pe12.4)')"iter: ",iter,", res: ",res
            if(hecmesh%my_rank==0) write(ista,'(''iter='',I5,''- Residual'',E15.7)')iter,res
            if( res<fstrsolid%step_ctrl(cstep)%converg ) exit
          endif
 
          !C-- linear solver [A]{X} = {B}
          hecmatmpc%X = 0.0d0
          if( iexit .ne. 1 ) then
            if( fstrparam%nlgeom ) then
              if( iter == 1 ) then
                hecmatmpc%Iarray(97) = 2   !Force numerical factorization
              else
                hecmatmpc%Iarray(97) = 1   !Need numerical factorization
              endif
              call fstr_set_current_config_to_mesh(hecmeshmpc,fstrsolid,coord)
            endif
            call solve_lineq(hecmeshmpc,hecmatmpc)
            if( fstrparam%nlgeom ) then
              call fstr_recover_initial_config_to_mesh(hecmeshmpc,fstrsolid,coord)
            endif
          endif
          call hecmw_mpc_tback_sol(hecmesh, hecmat, hecmatmpc)
 
          do j=1,hecmesh%n_node*ndof
            fstrsolid%dunode(j)  = fstrsolid%dunode(j)+hecmat%X(j)
          enddo
          ! ----- update the strain, stress, and internal force
          call fstr_updatenewton( hecmesh, hecmat, fstrsolid, fstrdynamic%t_curr, &
            &  fstrdynamic%t_delta, iter, fstrdynamic%strainEnergy )
 
          if(.not. fstrparam%nlgeom) exit
          if(ndof == 4) exit
        enddo
 
        ! -----  not convergence
        if( iter>fstrsolid%step_ctrl(cstep)%max_iter ) then
          if( hecmesh%my_rank==0) then
            write(ilog,*) '### Fail to Converge  : at step=', i
            write(ista,*) '### Fail to Converge  : at step=', i
            write(   *,*) '     ### Fail to Converge  : at step=', i
          endif
          stop
        endif
 
        !C *****************************************************
        !C for couple analysis
        call fstr_solve_dynamic_nlimplicit_couple_post(hecmesh, hecmat, fstrsolid, &
          & fstrparam, fstrdynamic, fstrcpl, a1, a2, a3, b1, b2, b3, i, is_cycle)
        if(is_cycle) cycle
        exit
      enddo
      !C *****************************************************
 
      !C-- new displacement, velocity and acceleration
      fstrdynamic%kineticEnergy = 0.0d0
      do j = 1 ,ndof*nnod
        fstrdynamic%ACC (j,2) = -a1*fstrdynamic%ACC(j,1) - a2*fstrdynamic%VEL(j,1) + &
          a3*fstrsolid%dunode(j)
        fstrdynamic%VEL (j,2) = -b1*fstrdynamic%ACC(j,1) - b2*fstrdynamic%VEL(j,1) + &
          b3*fstrsolid%dunode(j)
        fstrdynamic%ACC (j,1) = fstrdynamic%ACC (j,2)
        fstrdynamic%VEL (j,1) = fstrdynamic%VEL (j,2)
 
        fstrsolid%unode(j)  = fstrsolid%unode(j)+fstrsolid%dunode(j)
        fstrdynamic%DISP(j,2) = fstrsolid%unode(j)
 
        fstrdynamic%kineticEnergy = fstrdynamic%kineticEnergy + &
          0.5d0*fstreig%mass(j)*fstrdynamic%VEL(j,2)*fstrdynamic%VEL(j,2)
      enddo
 
      !C-- output new displacement, velocity and acceleration
      call fstr_dynamic_output(hecmesh, fstrsolid, fstrdynamic, fstrparam)
 
      !C-- output result of monitoring node
      call dynamic_output_monit(hecmesh, fstrparam, fstrdynamic, fstreig, fstrsolid)
      call fstr_updatestate( hecmesh, fstrsolid, fstrdynamic%t_delta )
 
     !---  Restart info
      if( fstrdynamic%restart_nout > 0 ) then
        if( mod(i,fstrdynamic%restart_nout).eq.0 .or. i.eq.fstrdynamic%n_step) then
          call fstr_write_restart_dyna_nl(i,hecmesh,fstrsolid,fstrdynamic,fstrparam)
        endif
      endif
 
    enddo
    !C-- end of time step loop
    time_2 = hecmw_wtime()
 
    if( hecmesh%my_rank == 0 ) then
      write(ista,'(a,f10.2,a)') '         solve (sec) :', time_2 - time_1, 's'
    endif
 
    deallocate(coord)
    call hecmw_mpc_mat_finalize(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
    if (associated(hecmat0)) then
      call hecmw_mat_finalize(hecmat0)
      deallocate(hecmat0)
    endif
  end subroutine fstr_solve_dynamic_nlimplicit
 
  subroutine fstr_solve_dynamic_nlimplicit_contactslag(cstep, hecMESH,hecMAT,fstrSOLID,fstrEIG   &
      ,fstrDYNAMIC,fstrRESULT,fstrPARAM &
      ,fstrCPL,hecLagMAT,restrt_step_num,infoCTChange  &
      ,conMAT )
    implicit none
    !C-- global variable
    integer, intent(in)                  :: cstep
    type(hecmwst_local_mesh)             :: hecMESH
    type(hecmwst_matrix)                 :: hecMAT
    type(hecmwst_matrix), pointer        :: hecMAT0
    type(fstr_eigen)                     :: fstrEIG
    type(fstr_solid)                     :: fstrSOLID
    type(hecmwst_result_data)            :: fstrRESULT
    type(fstr_param)                     :: fstrPARAM
    type(fstr_dynamic)                   :: fstrDYNAMIC
    type(fstr_couple)                    :: fstrCPL !for COUPLE
    type(hecmwst_matrix_lagrange)        :: hecLagMAT
    type(fstr_info_contactchange)        :: infoCTChange
    type(hecmwst_matrix)                 :: conMAT
 
    !C-- local variable
    integer(kind=kint) :: nnod, ndof, numnp, nn
    integer(kind=kint) :: i, j, ids, ide, ims, ime, kk, idm, imm
    integer(kind=kint) :: iter
    real(kind=kreal) :: a1, a2, a3, b1, b2, b3, c1, c2
    real(kind=kreal) :: bsize, res, res1, rf
    real(kind=kreal) :: res0, relres
    real :: time_1, time_2
    real(kind=kreal), parameter :: pi = 3.14159265358979323846d0
 
    integer(kind=kint) :: restrt_step_num
    integer(kind=kint) :: ctAlgo
    integer(kind=kint) :: max_iter_contact, count_step
    integer(kind=kint) :: stepcnt
    real(kind=kreal)   :: maxdlag, converg_dlag
 
    integer(kind=kint) :: n_node_global
    integer(kind=kint) :: contact_changed_global
    integer(kind=kint) ::  nndof,npdof
    logical :: is_mat_symmetric
    integer :: istat
    logical :: is_cycle
    real(kind=kreal),allocatable :: tmp_conb(:)
    real(kind=kreal), allocatable :: coord(:)
 
    nullify(hecmat0)
 
    ! sum of n_node among all subdomains (to be used to calc res)
    n_node_global = hecmesh%nn_internal
    call hecmw_allreduce_i1(hecmesh,n_node_global,hecmw_sum)
 
    ctalgo = fstrparam%contact_algo
 
    if( hecmat%Iarray(99)==4 .and. .not.fstr_is_matrixstruct_symmetric(fstrsolid,hecmesh) ) then
      write(*,*) ' This type of direct solver is not yet available in such case ! '
      write(*,*) ' Please use intel MKL direct solver !'
      call  hecmw_abort(hecmw_comm_get_comm())
    endif
 
    hecmat%NDOF=hecmesh%n_dof
 
    nnod=hecmesh%n_node
    ndof=hecmat%NDOF
    nn=ndof*ndof
 
    allocate(coord(hecmesh%n_node*ndof))
    if( associated( fstrsolid%contacts ) ) call initialize_contact_output_vectors(fstrsolid,hecmat)
 
    !!-- initial value
    time_1 = hecmw_wtime()
 
    !C-- check parameters
    if(dabs(fstrdynamic%beta) < 1.0e-20) then
      if( hecmesh%my_rank == 0 ) then
        write(imsg,*) 'stop due to Newmark-beta = 0'
      endif
      call hecmw_abort( hecmw_comm_get_comm())
    endif
 
    !C-- matrix [M] lumped mass matrix
    if(fstrdynamic%idx_mas == 1) then
      call setmass(fstrsolid,hecmesh,hecmat,fstreig)
 
    !C-- consistent mass matrix
    else if(fstrdynamic%idx_mas == 2) then
      if( hecmesh%my_rank .eq. 0 ) then
        write(imsg,*) 'stop: consistent mass matrix is not yet available !'
      endif
      call hecmw_abort( hecmw_comm_get_comm())
    endif
 
    hecmat%Iarray(98) = 1   !Assembly complete
    hecmat%Iarray(97) = 1   !Need numerical factorization
 
    !C-- initialize variables
    if( restrt_step_num == 1 .and. fstrdynamic%VarInitialize .and. fstrdynamic%ray_m /= 0.0d0 ) &
      call dynamic_init_varibles( hecmesh, hecmat, fstrsolid, fstreig, fstrdynamic, fstrparam )
 
    !C-- time step loop
    a1 = .5d0/fstrdynamic%beta - 1.d0
    a2 = 1.d0/(fstrdynamic%beta*fstrdynamic%t_delta)
    a3 = 1.d0/(fstrdynamic%beta*fstrdynamic%t_delta*fstrdynamic%t_delta)
    b1 = ( .5d0*fstrdynamic%gamma/fstrdynamic%beta - 1.d0 )*fstrdynamic%t_delta
    b2 = fstrdynamic%gamma/fstrdynamic%beta - 1.d0
    b3 = fstrdynamic%gamma/(fstrdynamic%beta*fstrdynamic%t_delta)
    c1 = 1.d0 + fstrdynamic%ray_k*b3
    c2 = a3 + fstrdynamic%ray_m*b3
 
    !C-- output of initial state
    if( restrt_step_num == 1 ) then
      call fstr_dynamic_output(hecmesh, fstrsolid, fstrdynamic, fstrparam)
      call dynamic_output_monit(hecmesh, fstrparam, fstrdynamic, fstreig, fstrsolid)
    endif
 
    fstrdynamic%VEC3(:) =0.d0
    hecmat%X(:) =0.d0
 
    call fstr_save_originalmatrixstructure(hecmat)
    call fstr_scan_contact_state(cstep, restrt_step_num, 0, fstrdynamic%t_delta, ctalgo, hecmesh, fstrsolid, infoctchange, hecmat%B)
 
    call hecmw_mat_copy_profile( hecmat, conmat )
 
    if ( fstr_is_contact_active() ) then
      call fstr_mat_con_contact( cstep, ctalgo, hecmat, fstrsolid, heclagmat, infoctchange, conmat, fstr_is_contact_active())
    elseif( hecmat%Iarray(99)==4 ) then
      write(*,*) ' This type of direct solver is not yet available in such case ! '
      write(*,*) ' Please change solver type to intel MKL direct solver !'
      call  hecmw_abort(hecmw_comm_get_comm())
    endif
    is_mat_symmetric = fstr_is_matrixstruct_symmetric(fstrsolid,hecmesh)
    call solve_lineq_contact_init(hecmesh,hecmat,heclagmat,is_mat_symmetric)
 
    max_iter_contact = fstrsolid%step_ctrl(cstep)%max_contiter
    converg_dlag = fstrsolid%step_ctrl(cstep)%converg_lag
    
    !! step = 1,2,....,fstrDYNAMIC%n_step
    do i = restrt_step_num, fstrdynamic%n_step
 
      fstrdynamic%i_step = i
      fstrdynamic%t_curr = fstrdynamic%t_delta * i
 
      if(hecmesh%my_rank==0) then
        write(ista,'('' time step='',i10,'' time='',1pe13.4e3)') i,fstrdynamic%t_curr
        write(*,'(A)')'-------------------------------------------------'
        write(*,'('' time step='',i10,'' time='',1pe13.4e3)') i,fstrdynamic%t_curr
      endif
 
      do j = 1 ,ndof*nnod
        fstrdynamic%VEC1(j) = a1*fstrdynamic%ACC(j,1) + a2*fstrdynamic%VEL(j,1)
        fstrdynamic%VEC2(j) = b1*fstrdynamic%ACC(j,1) + b2*fstrdynamic%VEL(j,1)
      enddo
 
      count_step = 0
      stepcnt = 0
 
      !C for couple analysis
      do
        fstrsolid%dunode(:) =0.d0
        ! call fstr_UpdateEPState( hecMESH, fstrSOLID )
        call fstr_solve_dynamic_nlimplicit_couple_init(fstrparam, fstrcpl)
 
      loopforcontactanalysis: do while( .true. )
      count_step = count_step + 1
 
        ! ----- Inner Iteration
        res0   = 0.d0
        res1   = 0.d0
        relres = 1.d0
 
        do iter = 1, fstrsolid%step_ctrl(cstep)%max_iter
          stepcnt=stepcnt+1
          if (fstrparam%nlgeom) then
            call fstr_stiffmatrix( hecmesh, hecmat, fstrsolid, fstrdynamic%t_curr, fstrdynamic%t_delta )
          else
            if (.not. associated(hecmat0)) then
              call fstr_stiffmatrix( hecmesh, hecmat, fstrsolid, fstrdynamic%t_curr, fstrdynamic%t_delta )
              allocate(hecmat0)
              call hecmw_mat_init(hecmat0)
              call hecmw_mat_copy_profile(hecmat, hecmat0)
              call hecmw_mat_copy_val(hecmat, hecmat0)
            else
              call hecmw_mat_copy_val(hecmat0, hecmat)
            endif
          endif
 
          if( fstrdynamic%ray_k/=0.d0 .or. fstrdynamic%ray_m/=0.d0 ) then
            do j = 1 ,ndof*nnod
              hecmat%X(j) = fstrdynamic%VEC2(j) - b3*fstrsolid%dunode(j)
            enddo
          endif
          if( fstrdynamic%ray_k/=0.d0 ) then
            if( hecmesh%n_dof == 3 ) then
              call hecmw_matvec (hecmesh, hecmat, hecmat%X, fstrdynamic%VEC3)
            else if( hecmesh%n_dof == 2 ) then
              call hecmw_matvec (hecmesh, hecmat, hecmat%X, fstrdynamic%VEC3)
            else if( hecmesh%n_dof == 6 ) then
              call matvec(fstrdynamic%VEC3, hecmat%X, hecmat, ndof, hecmat%D, hecmat%AU, hecmat%AL)
            endif
          endif
 
          !C-- mechanical boundary condition
          call dynamic_mat_ass_load (hecmesh, hecmat, fstrsolid, fstrdynamic, fstrparam)
          do j=1, hecmesh%n_node*  hecmesh%n_dof
            hecmat%B(j)=hecmat%B(j)- fstrsolid%QFORCE(j) + fstreig%mass(j)*( fstrdynamic%VEC1(j)-a3*fstrsolid%dunode(j)   &
              + fstrdynamic%ray_m* hecmat%X(j) ) + fstrdynamic%ray_k*fstrdynamic%VEC3(j)
          enddo
 
          !C for couple analysis
          call fstr_solve_dynamic_nlimplicit_couple_pre(hecmesh, hecmat, fstrsolid, &
            & fstrparam, fstrdynamic, fstrcpl, restrt_step_num, pi, i)
 
          do j = 1 ,nn*hecmat%NP
            hecmat%D(j)  = c1* hecmat%D(j)
          enddo
          do j = 1 ,nn*hecmat%NPU
            hecmat%AU(j) = c1* hecmat%AU(j)
          enddo
          do j = 1 ,nn*hecmat%NPL
            hecmat%AL(j) = c1*hecmat%AL(j)
          enddo
          do j=1,nnod
            do kk=1,ndof
              idm = nn*(j-1)+1 + (ndof+1)*(kk-1)
              imm = ndof*(j-1) + kk
              hecmat%D(idm) = hecmat%D(idm) + c2*fstreig%mass(imm)
            enddo
          enddo
 
          call hecmw_mat_clear( conmat )
          call hecmw_mat_clear_b( conmat )
          conmat%X = 0.0d0
 
          if( fstr_is_contact_active() ) then
            call fstr_update_ndforce_contact(cstep,hecmesh,hecmat,heclagmat,fstrsolid,conmat)
            call fstr_addcontactstiffness(cstep,iter,conmat,heclagmat,fstrsolid)
          endif
 
          !C-- geometrical boundary condition
          call dynamic_mat_ass_bc   (hecmesh, hecmat, fstrsolid, fstrdynamic, fstrparam, heclagmat, stepcnt, conmat=conmat)
          call dynamic_mat_ass_bc_vl(hecmesh, hecmat, fstrsolid, fstrdynamic, fstrparam, heclagmat, stepcnt, conmat=conmat)
          call dynamic_mat_ass_bc_ac(hecmesh, hecmat, fstrsolid, fstrdynamic, fstrparam, heclagmat, stepcnt, conmat=conmat)
 
          ! ----- check convergence
          res = fstr_get_norm_para_contact(hecmat,heclagmat,conmat,hecmesh)
 
          if(iter == 1)then
            res0 = res
          endif
 
          ! ----- check convergence
          if( .not.fstr_is_contact_active() ) then
            maxdlag = 0.0d0
          elseif( maxdlag  == 0.0d0) then
            maxdlag = 1.0d0
          endif
          call hecmw_allreduce_r1(hecmesh, maxdlag, hecmw_max)
 
          res = dsqrt(res/res0)
          if( hecmesh%my_rank==0 ) then
              if(hecmesh%my_rank==0) write(*,'(a,i5,a,1pe12.4)')"iter: ",iter,", res: ",res
            if(hecmesh%my_rank==0) write(ista,'(''iter='',I5,''- Residual'',E15.7)')iter,res
            write(*,'(a,1e15.7)') ' - MaxDLag =',maxdlag
            write(ista,'(a,1e15.7)') ' - MaxDLag =',maxdlag
          endif
          if( res<fstrsolid%step_ctrl(cstep)%converg .and. maxdlag < converg_dlag ) exit
 
          !   ----  For Parallel Contact with Multi-Partition Domains
          hecmat%X = 0.0d0
          call fstr_set_current_config_to_mesh(hecmesh,fstrsolid,coord)
          call solve_lineq_contact(hecmesh,hecmat,heclagmat,conmat,istat,1.0d0,fstr_is_contact_active())
          call fstr_recover_initial_config_to_mesh(hecmesh,fstrsolid,coord)
 
          ! ----- update external nodal displacement increments
          call hecmw_update_r (hecmesh, hecmat%X, hecmat%NP, hecmat%NDOF)
 
          ! ----- update the strain, stress, and internal force
          do j=1,hecmesh%n_node*ndof
            fstrsolid%dunode(j)  = fstrsolid%dunode(j)+hecmat%X(j)
          enddo
          call fstr_updatenewton( hecmesh, hecmat, fstrsolid, fstrdynamic%t_curr, &
            &   fstrdynamic%t_delta,iter, fstrdynamic%strainEnergy )
 
          if(.not. fstrparam%nlgeom) exit
 
          ! ----- update the Lagrange multipliers
          if( fstr_is_contact_active() ) then
            maxdlag = 0.0d0
            do j=1,heclagmat%num_lagrange
              heclagmat%lagrange(j) = heclagmat%lagrange(j) + hecmat%X(hecmesh%n_node*ndof+j)
              if(dabs(hecmat%X(hecmesh%n_node*ndof+j))>maxdlag) maxdlag=dabs(hecmat%X(hecmesh%n_node*ndof+j))
              !              write(*,*)'Lagrange:', j,hecLagMAT%lagrange(j),hecMAT%X(hecMESH%n_node*ndof+j)
            enddo
          endif
        enddo
 
        ! -----  not convergence
        if( iter>fstrsolid%step_ctrl(cstep)%max_iter ) then
          if( hecmesh%my_rank==0) then
            write(ilog,*) '### Fail to Converge  : at step=', i
            write(ista,*) '### Fail to Converge  : at step=', i
            write(   *,*) '     ### Fail to Converge  : at step=', i
          endif
          stop
        endif
 
        call fstr_scan_contact_state(cstep, i, count_step, fstrdynamic%t_delta, ctalgo, hecmesh, fstrsolid, infoctchange, hecmat%B)
 
        if( hecmat%Iarray(99)==4 .and. .not. fstr_is_contact_active() ) then
          write(*,*) ' This type of direct solver is not yet available in such case ! '
          write(*,*) ' Please use intel MKL direct solver !'
          call  hecmw_abort(hecmw_comm_get_comm())
        endif
 
        is_mat_symmetric = fstr_is_matrixstruct_symmetric(fstrsolid,hecmesh)
        contact_changed_global=0
        if( fstr_is_contact_conv(ctalgo,infoctchange,hecmesh) ) then
          exit loopforcontactanalysis
        elseif( fstr_is_matrixstructure_changed(infoctchange) ) then
          call fstr_mat_con_contact( cstep, ctalgo, hecmat, fstrsolid, heclagmat, infoctchange, conmat, fstr_is_contact_active())
          contact_changed_global=1
        endif
        call hecmw_allreduce_i1(hecmesh,contact_changed_global,hecmw_max)
        if (contact_changed_global > 0) then
          call hecmw_mat_clear_b( hecmat )
          call hecmw_mat_clear_b( conmat )
          call solve_lineq_contact_init(hecmesh,hecmat,heclagmat,is_mat_symmetric)
        endif
 
        if( count_step > max_iter_contact ) exit loopforcontactanalysis
 
      enddo loopforcontactanalysis
 
      !C for couple analysis
        call fstr_solve_dynamic_nlimplicit_couple_post(hecmesh, hecmat, fstrsolid, &
          & fstrparam, fstrdynamic, fstrcpl, a1, a2, a3, b1, b2, b3, i, is_cycle)
        if(is_cycle) cycle
        exit
      enddo
 
      !C-- new displacement, velocity and acceleration
      fstrdynamic%kineticEnergy = 0.0d0
      do j = 1 ,ndof*nnod
        fstrdynamic%ACC (j,2) = -a1*fstrdynamic%ACC(j,1) - a2*fstrdynamic%VEL(j,1) + &
          a3*fstrsolid%dunode(j)
        fstrdynamic%VEL (j,2) = -b1*fstrdynamic%ACC(j,1) - b2*fstrdynamic%VEL(j,1) + &
          b3*fstrsolid%dunode(j)
        fstrdynamic%ACC (j,1) = fstrdynamic%ACC (j,2)
        fstrdynamic%VEL (j,1) = fstrdynamic%VEL (j,2)
 
        fstrsolid%unode(j)  = fstrsolid%unode(j)+fstrsolid%dunode(j)
        fstrdynamic%DISP(j,2) = fstrsolid%unode(j)
 
        fstrdynamic%kineticEnergy = fstrdynamic%kineticEnergy + &
          0.5d0*fstreig%mass(j)*fstrdynamic%VEL(j,2)*fstrdynamic%VEL(j,2)
      enddo
 
      !C-- output new displacement, velocity and acceleration
      call fstr_dynamic_output(hecmesh, fstrsolid, fstrdynamic, fstrparam)
 
      !C-- output result of monitoring node
      call dynamic_output_monit(hecmesh, fstrparam, fstrdynamic, fstreig, fstrsolid)
 
      call fstr_updatestate( hecmesh, fstrsolid, fstrdynamic%t_delta )
 
      !---  Restart info
      if( fstrdynamic%restart_nout > 0 ) then
        if( mod(i,fstrdynamic%restart_nout).eq.0 .or. i.eq.fstrdynamic%n_step ) then
          call fstr_write_restart_dyna_nl(i,hecmesh,fstrsolid,fstrdynamic,fstrparam,&
            infoctchange%contactNode_current)
        endif
      endif
 
    enddo
    !C-- end of time step loop
 
    if (associated(hecmat0)) then
      call hecmw_mat_finalize(hecmat0)
      deallocate(hecmat0)
    endif
 
    time_2 = hecmw_wtime()
    if( hecmesh%my_rank == 0 ) then
      write(ista,'(a,f10.2,a)') '         solve (sec) :', time_2 - time_1, 's'
    endif
 
    deallocate(coord)
  end subroutine fstr_solve_dynamic_nlimplicit_contactslag
 
  subroutine fstr_solve_dynamic_nlimplicit_couple_init(fstrPARAM, fstrCPL)
    implicit none
    type(fstr_param)         :: fstrparam
    type(fstr_couple)        :: fstrCPL
    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 )
    endif
  end subroutine fstr_solve_dynamic_nlimplicit_couple_init
 
  subroutine fstr_solve_dynamic_nlimplicit_couple_pre(hecMESH, hecMAT, fstrSOLID, &
    & fstrPARAM, fstrDYNAMIC, fstrCPL, restrt_step_num, PI, i)
    implicit none
    type(hecmwst_local_mesh) :: hecMESH
    type(hecmwst_matrix)     :: hecMAT
    type(fstr_solid)         :: fstrSOLID
    type(fstr_param)         :: fstrPARAM
    type(fstr_dynamic)       :: fstrDYNAMIC
    type(fstr_couple)        :: fstrCPL
    integer(kint) :: kkk0, kkk1, j, kk, i, restrt_step_num
    real(kreal) :: bsize, PI
 
    if( fstrparam%fg_couple == 1) then
      if( fstrparam%fg_couple_first /= 0 ) then
        bsize = dfloat( i ) / 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 = i - restrt_step_num + 1
        kk = fstrdynamic%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
  end subroutine fstr_solve_dynamic_nlimplicit_couple_pre
 
  subroutine fstr_solve_dynamic_nlimplicit_couple_post(hecMESH, hecMAT, fstrSOLID, &
    & fstrPARAM, fstrDYNAMIC, fstrCPL, a1, a2, a3, b1, b2, b3, i, is_cycle)
    implicit none
    type(hecmwst_local_mesh) :: hecMESH
    type(hecmwst_matrix)     :: hecMAT
    type(fstr_solid)         :: fstrSOLID
    type(fstr_param)         :: fstrPARAM
    type(fstr_dynamic)       :: fstrDYNAMIC
    type(fstr_couple)        :: fstrCPL
    integer(kint) :: kkk0, kkk1, j, i, revocap_flag
    real(kreal) :: bsize, a1, a2, a3, b1, b2, b3
    logical :: is_cycle
 
    is_cycle = .false.
 
    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) = fstrsolid%unode(kkk1-2) + fstrsolid%dunode(kkk1-2)
            fstrcpl%disp (kkk0-1) = fstrsolid%unode(kkk1-1) + fstrsolid%dunode(kkk1-1)
            fstrcpl%disp (kkk0  ) = fstrsolid%unode(kkk1  ) + fstrsolid%dunode(kkk1  )
 
            fstrcpl%velo (kkk0-2) = -b1*fstrdynamic%ACC(kkk1-2,1) - b2*fstrdynamic%VEL(kkk1-2,1) + &
              b3*fstrsolid%dunode(kkk1-2)
            fstrcpl%velo (kkk0-1) = -b1*fstrdynamic%ACC(kkk1-1,1) - b2*fstrdynamic%VEL(kkk1-1,1) + &
              b3*fstrsolid%dunode(kkk1-1)
            fstrcpl%velo (kkk0  ) = -b1*fstrdynamic%ACC(kkk1,1) - b2*fstrdynamic%VEL(kkk1,1) + &
              b3*fstrsolid%dunode(kkk1)
            fstrcpl%accel(kkk0-2) = -a1*fstrdynamic%ACC(kkk1-2,1) - a2*fstrdynamic%VEL(kkk1-2,1) + &
              a3*fstrsolid%dunode(kkk1-2)
            fstrcpl%accel(kkk0-1) = -a1*fstrdynamic%ACC(kkk1-1,1) - a2*fstrdynamic%VEL(kkk1-1,1) + &
              a3*fstrsolid%dunode(kkk1-1)
            fstrcpl%accel(kkk0  ) = -a1*fstrdynamic%ACC(kkk1,1) - a2*fstrdynamic%VEL(kkk1,1) + &
              a3*fstrsolid%dunode(kkk1)
          else
            kkk0 = j*2
            kkk1 = fstrcpl%coupled_node(j)*2
 
            fstrcpl%disp (kkk0-1) = fstrsolid%unode(kkk1-1) + fstrsolid%dunode(kkk1-1)
            fstrcpl%disp (kkk0  ) = fstrsolid%unode(kkk1  ) + fstrsolid%dunode(kkk1  )
 
            fstrcpl%velo (kkk0-1) = -b1*fstrdynamic%ACC(kkk1-1,1) - b2*fstrdynamic%VEL(kkk1-1,1) + &
              b3*fstrsolid%dunode(kkk1-1)
            fstrcpl%velo (kkk0  ) = -b1*fstrdynamic%ACC(kkk1,1) - b2*fstrdynamic%VEL(kkk1,1) + &
              b3*fstrsolid%dunode(kkk1)
            fstrcpl%accel(kkk0-1) = -a1*fstrdynamic%ACC(kkk1-1,1) - a2*fstrdynamic%VEL(kkk1-1,1) + &
              a3*fstrsolid%dunode(kkk1-1)
            fstrcpl%accel(kkk0  ) = -a1*fstrdynamic%ACC(kkk1,1) - a2*fstrdynamic%VEL(kkk1,1) + &
              a3*fstrsolid%dunode(kkk1)
          endif
        enddo
        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 ) is_cycle = .true.
        case (6)
          call fstr_get_convergence( revocap_flag )
          if( revocap_flag==0 ) then
            call fstr_rcap_get( fstrcpl )
            is_cycle = .true.
          else
            if( i /= fstrdynamic%n_step ) call fstr_rcap_get( fstrcpl )
          endif
      end select
    endif
  end subroutine fstr_solve_dynamic_nlimplicit_couple_post
 
end module fstr_dynamic_nlimplicit