fstr_solve_NonLinear.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 m_fstr_nonlinearmethod
 
  use m_fstr
  use m_static_lib
  use m_static_output
 
  use m_fstr_spring
  use m_fstr_stiffmatrix
  use m_fstr_update
  use m_fstr_ass_load
  use m_fstr_addbc
  use m_fstr_residual
  use m_fstr_restart
 
  implicit none
 
  contains
 
 
  subroutine fstr_newton( cstep, hecMESH, hecMAT, fstrSOLID, fstrPARAM, &
      restrt_step_num, sub_step, ctime, dtime )
 
    integer, intent(in)                   :: cstep
    type (hecmwST_local_mesh)             :: hecMESH
    type (hecmwST_matrix)                 :: hecMAT
    type (fstr_solid)                     :: fstrSOLID
    integer, intent(in)                   :: sub_step
    real(kind=kreal), intent(in)          :: ctime     
    real(kind=kreal), intent(in)          :: dtime     
    type (fstr_param)                     :: fstrPARAM
    type (hecmwST_matrix_lagrange)        :: hecLagMAT
 
    type (hecmwST_local_mesh), pointer :: hecMESHmpc
    type (hecmwST_matrix), pointer :: hecMATmpc
    integer(kind=kint) :: ndof
    integer(kind=kint) :: i, iter
    integer(kind=kint) :: stepcnt
    integer(kind=kint) :: restrt_step_num
    real(kind=kreal)   :: tt0, tt, res, qnrm, rres, tincr, xnrm, dunrm, rxnrm
    real(kind=kreal), allocatable :: coord(:), p(:)
    logical :: isLinear = .false.
    integer(kind=kint) :: iterStatus
 
    call hecmw_mpc_mat_init(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
 
    if(.not. fstrpr%nlgeom)then
      islinear = .true.
    endif
 
    call fstr_init_newton(hecmesh, hecmat, fstrsolid, ctime, tincr, iter, cstep, dtime, fstrparam, heclagmat, ndof)
 
    allocate(p(hecmesh%n_node*ndof))
    allocate(coord(hecmesh%n_node*ndof))
    p = 0.0d0
    stepcnt = 0
 
    ! ----- Inner Iteration, lagrange multiplier constant
    do iter=1,fstrsolid%step_ctrl(cstep)%max_iter
      stepcnt = stepcnt+1
 
      call fstr_stiffmatrix( hecmesh, hecmat, fstrsolid, ctime, tincr )
      call fstr_addspring(cstep, hecmesh, hecmat, fstrsolid, fstrparam)
 
      ! ----- Set Boundary condition
      call fstr_addbc(cstep, hecmesh, hecmat, fstrsolid, fstrparam, heclagmat, stepcnt)
      call hecmw_mpc_mat_ass(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
      call hecmw_mpc_trans_rhs(hecmesh, hecmat, hecmatmpc)
 
      !----- SOLVE [Kt]{du}={R}
      if( sub_step == restrt_step_num .and. iter == 1 ) hecmatmpc%Iarray(98) = 1
      if( iter == 1 ) then
        hecmatmpc%Iarray(97) = 2   !Force numerical factorization
      else
        hecmatmpc%Iarray(97) = 1   !Need numerical factorization
      endif
      hecmatmpc%X = 0.0d0
      call fstr_set_current_config_to_mesh(hecmeshmpc,fstrsolid,coord)
      call solve_lineq(hecmeshmpc,hecmatmpc)
      call fstr_recover_initial_config_to_mesh(hecmeshmpc,fstrsolid,coord)
      call hecmw_mpc_tback_sol(hecmesh, hecmat, hecmatmpc)
 
      ! ----- update the small displacement and the displacement for 1step
      !       \delta u^k => solver's solution
      !       \Delta u_{n+1}^{k} = \Delta u_{n+1}^{k-1} + \delta u^k
      do i = 1, hecmesh%n_node*ndof
        fstrsolid%dunode(i) = fstrsolid%dunode(i) + hecmat%X(i)
      enddo
 
      call fstr_calc_residual_vector(hecmesh, hecmat, fstrsolid, ctime, tincr, iter, cstep, dtime, fstrparam)
 
      ! do i = 1, hecMESH%n_node*ndof
      !   write(6,*) fstrSOLID%dunode(i), fstrSOLID%QFORCE(i), fstrSOLID%GL(i)
      ! enddo
 
      if( islinear ) exit
 
      ! ----- check convergence
      iterstatus = fstr_check_iteration_converged(hecmesh, hecmat, fstrsolid, ndof, iter, sub_step, cstep)
      if (iterstatus == kitrconverged) exit
      if (iterstatus == kitrdiverged .or. iterstatus==kitrfloatingerror) return
    enddo
    ! ----- end of inner loop
 
    fstrsolid%NRstat_i(knstmaxit) = max(fstrsolid%NRstat_i(knstmaxit),iter) ! logging newton iteration(maxtier)
    fstrsolid%NRstat_i(knstsumit) = fstrsolid%NRstat_i(knstsumit) + iter    ! logging newton iteration(sum of iter)
 
    ! ----- update the total displacement
    ! u_{n+1} = u_{n} + \Delta u_{n+1}
    do i=1,hecmesh%n_node*ndof
      fstrsolid%unode(i) = fstrsolid%unode(i) + fstrsolid%dunode(i)
    enddo
 
    call fstr_updatestate( hecmesh, fstrsolid, tincr )
 
    fstrsolid%CutBack_stat = 0
    deallocate(coord)
    deallocate(p)
    call hecmw_mpc_mat_finalize(hecmesh, hecmat, hecmeshmpc, hecmatmpc)
  end subroutine fstr_newton
 
  subroutine fstr_newton_contactalag( cstep, hecMESH, hecMAT, fstrSOLID, fstrPARAM,                   &
      restart_step_num, restart_substep_num, sub_step, ctime, dtime, infoCTChange, conMAT )
    use mcontact
    use m_addcontactstiffness
    use m_solve_lineq_contact
 
    integer, intent(in)                   :: cstep
    type (hecmwST_local_mesh)             :: hecMESH
    type (hecmwST_matrix)                 :: hecMAT
    type (fstr_solid)                     :: fstrSOLID
    integer, intent(in)                   :: sub_step
    real(kind=kreal), intent(in)          :: ctime     
    real(kind=kreal), intent(in)          :: dtime     
    type (fstr_param)                     :: fstrPARAM
    type (fstr_info_contactChange)        :: infoCTChange
    type (hecmwST_matrix_lagrange)        :: hecLagMAT
    type (hecmwST_matrix)                 :: conMAT
 
    integer(kind=kint) :: ndof
    integer(kind=kint) :: ctAlgo
    integer(kind=kint) :: i, iter
    integer(kind=kint) :: al_step, n_al_step, stepcnt
    real(kind=kreal)   :: tt0, tt, res, res0, res1, maxv, relres, tincr
    integer(kind=kint) :: restart_step_num, restart_substep_num
    logical            :: convg, ctchange
    integer(kind=kint) :: n_node_global
    integer(kind=kint) :: contact_changed_global
    real(kind=kreal), allocatable :: coord(:)
    integer(kind=kint)  :: istat
 
 
    ! 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
 
    call fstr_init_newton(hecmesh, hecmat, fstrsolid, ctime, tincr, iter, cstep, dtime, fstrparam, heclagmat, ndof)
 
    if( cstep == 1 .and. sub_step == restart_substep_num ) then
      call fstr_save_originalmatrixstructure(hecmat)
      if(hecmesh%my_rank==0) write(*,*) "---Scanning initial contact state---"
      call fstr_scan_contact_state( cstep, sub_step, 0, dtime, 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 the solver type to intel MKL direct solver !'
        call hecmw_abort(hecmw_comm_get_comm())
      endif
      call solve_lineq_contact_init(hecmesh, hecmat, heclagmat, .true.)
    endif
 
    hecmat%X = 0.0d0
 
    stepcnt = 0
    allocate(coord(hecmesh%n_node*ndof))
 
    call hecmw_mat_clear_b(conmat)
 
    if( fstr_is_contact_active() ) call fstr_ass_load_contactalag( hecmesh, fstrsolid, conmat%B )
 
    ! ----- Augmentation loop. In case of no contact, it is inactive
    n_al_step = fstrsolid%step_ctrl(cstep)%max_contiter
    if( .not. fstr_is_contact_active() ) n_al_step = 1
 
    do al_step = 1, n_al_step
 
      if( hecmesh%my_rank == 0) then
        if( n_al_step > 1 ) then
          print *, "Augmentation step:", al_step
          write(imsg, *) "Augmentation step:", al_step
        endif
      end if
 
      ! ----- Inner Iteration, lagrange multiplier constant
      res0   = 0.0d0
      res1   = 0.0d0
      relres = 1.0d0
 
      do iter = 1,fstrsolid%step_ctrl(cstep)%max_iter
        stepcnt = stepcnt+1
 
        call fstr_stiffmatrix( hecmesh, hecmat, fstrsolid, ctime, tincr )
        call fstr_addspring(cstep, hecmesh, hecmat, fstrsolid, fstrparam)
 
        call hecmw_mat_clear( conmat )
        conmat%X = 0.0d0
 
        ! ----- Contact
        if( al_step == 1 .and. stepcnt == 1 ) then
          maxv = hecmw_mat_diag_max( hecmat, hecmesh )
          call fstr_set_contact_penalty( maxv )
        endif
        if( fstr_is_contact_active() )  then
          call fstr_contactbc( cstep, iter, hecmesh, conmat, fstrsolid )
        endif
 
        ! ----- Set Boundary condition
        call fstr_addbc(cstep, hecmesh, hecmat, fstrsolid, fstrparam, heclagmat, stepcnt, conmat)
 
        !----- SOLVE [Kt]{du}={R}
        ! ----  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)
 
        call hecmw_update_r (hecmesh, hecmat%X, hecmat%NP, hecmesh%n_dof)
 
        ! ----- update the small displacement and the displacement for 1step
        !       \delta u^k => solver's solution
        !       \Delta u_{n+1}^{k} = \Delta u_{n+1}^{k-1} + \delta u^k
        do i = 1, hecmesh%n_node*ndof
          fstrsolid%dunode(i) = fstrsolid%dunode(i)+hecmat%X(i)
        enddo
 
        ! ----- update the strain, stress, and internal force
        call fstr_updatenewton(hecmesh, hecmat, fstrsolid, ctime, tincr, iter)
 
        ! ----- Set residual
        if( fstrsolid%DLOAD_follow /= 0 .or. fstrsolid%CLOAD_ngrp_rot /= 0 ) &
          call fstr_ass_load(cstep, ctime+dtime, hecmesh, hecmat, fstrsolid, fstrparam)
 
        call fstr_update_ndforce(cstep, hecmesh, hecmat, fstrsolid, conmat)
 
        if( fstr_is_contact_active() ) then
          call hecmw_mat_clear_b( conmat )
          call fstr_update_contact0(hecmesh, fstrsolid, conmat%B)
        endif
        !    Consider SPC condition
        call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, hecmat%B)
        call fstr_update_ndforce_spc(cstep, hecmesh, fstrsolid, conmat%B)
    
        !res = fstr_get_residual(hecMAT%B, hecMESH)
        res = fstr_get_norm_para_contact(hecmat,heclagmat,conmat,hecmesh)
        ! ----- Gather global residual
        res = sqrt(res)/n_node_global
        if( iter == 1 ) res0 = res
        if( res0 == 0.0d0 ) then
          res0 = 1.0d0
        else
          relres = dabs( res1-res )/res0
        endif
 
        if( hecmesh%my_rank == 0 ) then
          write(*, '(a,i3,a,2e15.7)') ' - Residual(',iter,') =', res, relres
        endif
 
        ! ----- check convergence
        if( res < fstrsolid%step_ctrl(cstep)%converg  .or.     &
          relres < fstrsolid%step_ctrl(cstep)%converg_ddisp ) exit
        res1 = res
 
        ! ----- check divergence and NaN
        if( iter == fstrsolid%step_ctrl(cstep)%max_iter .or. res > fstrsolid%step_ctrl(cstep)%maxres .or. res /= res ) then
          if( hecmesh%my_rank == 0) then
            write(   *,'(a,i5,a,i5)') '     ### Fail to Converge  : at total_step=', cstep, '  sub_step=', sub_step
          end if
          fstrsolid%NRstat_i(knstmaxit) = max(fstrsolid%NRstat_i(knstmaxit),iter) ! logging newton iteration(maxtier)
          fstrsolid%NRstat_i(knstsumit) = fstrsolid%NRstat_i(knstsumit) + iter    ! logging newton iteration(sumofiter)
          fstrsolid%NRstat_i(knstciter) = al_step                                 ! logging contact iteration
          fstrsolid%CutBack_stat = fstrsolid%CutBack_stat + 1
          if( iter == fstrsolid%step_ctrl(cstep)%max_iter ) fstrsolid%NRstat_i(knstdresn) = 1
          if( res > fstrsolid%step_ctrl(cstep)%maxres .or. res /= res ) fstrsolid%NRstat_i(knstdresn) = 2
          return
        end if
 
      enddo
      ! ----- end of inner loop
 
      fstrsolid%NRstat_i(knstmaxit) = max(fstrsolid%NRstat_i(knstmaxit),iter) ! logging newton iteration(maxtier)
      fstrsolid%NRstat_i(knstsumit) = fstrsolid%NRstat_i(knstsumit) + iter    ! logging newton iteration(sum of iter)
 
      ! ----- deal with contact boundary
      call fstr_update_contact_multiplier( hecmesh, fstrsolid, ctchange )
      call fstr_scan_contact_state( cstep, sub_step, al_step, dtime, ctalgo, hecmesh, fstrsolid, infoctchange, hecmat%B )
 
      contact_changed_global = 0
      if( 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, .true.)
      endif
 
      if( fstr_is_contact_conv(ctalgo,infoctchange,hecmesh) .and. .not. ctchange ) exit
 
      ! ----- check divergence
      if( al_step >= fstrsolid%step_ctrl(cstep)%max_contiter ) then
        if( hecmesh%my_rank == 0) then
          write(   *,'(a,i5,a,i5)') '     ### Contact failed to Converge  : at total_step=', cstep, '  sub_step=', sub_step
        end if
        fstrsolid%NRstat_i(knstciter) = al_step                              ! logging contact iteration
        fstrsolid%CutBack_stat = fstrsolid%CutBack_stat + 1
        fstrsolid%NRstat_i(knstdresn) = 3
        return
      end if
 
      ! ----- Set residual for next newton iteration
      call fstr_update_ndforce(cstep,hecmesh,hecmat,fstrsolid,conmat )
 
      if( fstr_is_contact_active() )  then
        call hecmw_mat_clear_b( conmat )
        call fstr_update_contact0(hecmesh, fstrsolid, conmat%B)
      endif
 
    enddo
    ! ----- end of augmentation loop
 
    ! ----- update the total displacement
    ! u_{n+1} = u_{n} + \Delta u_{n+1}
    do i=1,hecmesh%n_node*ndof
      fstrsolid%unode(i) = fstrsolid%unode(i)+fstrsolid%dunode(i)
    enddo
 
    fstrsolid%NRstat_i(knstciter) = al_step-1 ! logging contact iteration
 
    call fstr_updatestate( hecmesh, fstrsolid, tincr )
 
    deallocate(coord)
    fstrsolid%CutBack_stat = 0
  end subroutine fstr_newton_contactalag
 
 
  subroutine fstr_newton_contactslag( cstep, hecMESH, hecMAT, fstrSOLID, fstrPARAM, hecLagMAT,                  &
      restart_step_num, restart_substep_num, sub_step, ctime, dtime, infoCTChange, conMAT )
 
    use mcontact
    use m_addcontactstiffness
    use m_solve_lineq_contact
 
    integer, intent(in)                    :: cstep
    type (hecmwST_local_mesh)              :: hecMESH
    type (hecmwST_matrix)                  :: hecMAT
    type (fstr_solid)                      :: fstrSOLID
    integer, intent(in)                    :: sub_step
    real(kind=kreal), intent(in)           :: ctime     
    real(kind=kreal), intent(in)           :: dtime     
    type (fstr_param)                      :: fstrPARAM
    type (fstr_info_contactChange)         :: infoCTChange
    type (hecmwST_matrix_lagrange)         :: hecLagMAT
    type (hecmwST_matrix)                  :: conMAT
 
    integer(kind=kint) :: ndof
    integer(kind=kint) :: ctAlgo
    integer(kind=kint) :: i, iter, max_iter_contact
    integer(kind=kint) :: stepcnt, count_step
    real(kind=kreal)   :: tt0, tt, res, res0, res1, relres, tincr, resx
    integer(kind=kint) :: restart_step_num, restart_substep_num
    logical            :: is_mat_symmetric
    integer(kind=kint) :: n_node_global
    integer(kind=kint) :: contact_changed_global
    integer(kint)      :: nndof
    real(kreal)        :: q_residual,x_residual
    real(kind=kreal), allocatable :: coord(:)
    integer(kind=kint)  :: istat
 
 
    ! 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)
 
    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
 
    call fstr_init_newton(hecmesh, hecmat, fstrsolid, ctime, tincr, iter, cstep, dtime, fstrparam, heclagmat, ndof)
 
    ctalgo = fstrparam%contact_algo
    if( cstep==1 .and. sub_step==restart_substep_num  ) then
      call fstr_save_originalmatrixstructure(hecmat)
      call fstr_scan_contact_state( cstep, sub_step, 0, dtime, 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 the 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)
    endif
 
    call hecmw_mat_clear_b(conmat)
 
    if( fstr_is_contact_active() )  then
      call fstr_ass_load_contact(cstep, hecmesh, conmat, fstrsolid, heclagmat)
    endif
 
    stepcnt = 0
    count_step = 0
    allocate(coord(hecmesh%n_node*ndof))
 
    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
        call hecmw_barrier(hecmesh)
        if( myrank == 0 ) print *,'-------------------------------------------------'
        call hecmw_barrier(hecmesh)
        stepcnt = stepcnt+1
 
        call fstr_stiffmatrix(hecmesh, hecmat, fstrsolid, ctime, tincr)
        call fstr_addspring(cstep, hecmesh, hecmat, fstrsolid, fstrparam)
 
        call hecmw_mat_clear( conmat )
        conmat%X = 0.0d0
 
        if( fstr_is_contact_active() ) then
          call fstr_addcontactstiffness(cstep,iter,conmat,heclagmat,fstrsolid)
        endif
 
        ! ----- Set Boundary condition
        call fstr_addbc(cstep, hecmesh, hecmat, fstrsolid, fstrparam, heclagmat, stepcnt, conmat)
 
        nndof = hecmat%N*hecmat%ndof
 
        !----- SOLVE [Kt]{du}={R}
        ! ----  For Parallel Contact with Multi-Partition Domains
        hecmat%X = 0.0d0
        call fstr_set_current_config_to_mesh(hecmesh,fstrsolid,coord)
        q_residual = fstr_get_norm_para_contact(hecmat,heclagmat,conmat,hecmesh)
        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)
        ! ----- check matrix solver error
        if( istat /= 0 ) then
          if( hecmesh%my_rank == 0) then
            write(   *,'(a,i5,a,i5)') '     ### Fail to Converge  : at total_step=', cstep, '  sub_step=', sub_step
          end if
          fstrsolid%NRstat_i(knstdresn) = 4
          fstrsolid%CutBack_stat = fstrsolid%CutBack_stat + 1
          return
        end if
 
        x_residual = fstr_get_x_norm_contact(hecmat,heclagmat,hecmesh)
 
        call hecmw_innerproduct_r(hecmesh,ndof,hecmat%X,hecmat%X,resx)
        resx = sqrt(resx)/n_node_global
 
        if( hecmesh%my_rank==0 ) then
          write(*,'(a,i3,a,e15.7)') ' - ResidualX    (',iter,') =',resx
          write(*,'(a,i3,a,e15.7)') ' - ResidualX+LAG(',iter,') =',sqrt(x_residual)/n_node_global
          write(*,'(a,i3,a,e15.7)') ' - ResidualQ    (',iter,') =',sqrt(q_residual)/n_node_global
        endif
 
        ! ----- update the small displacement and the displacement for 1step
        do i = 1, hecmesh%n_node*ndof
          fstrsolid%dunode(i) = fstrsolid%dunode(i) + hecmat%X(i)
        enddo
 
        ! ----- update the Lagrange multipliers
        if( fstr_is_contact_active() ) then
          do i = 1, heclagmat%num_lagrange
            heclagmat%lagrange(i) = heclagmat%lagrange(i)+hecmat%X(hecmesh%n_node*ndof+i)
          enddo
        endif
 
        ! ----- update the strain, stress, and internal force (only QFORCE)
        call fstr_updatenewton(hecmesh, hecmat, fstrsolid, ctime, tincr, iter)
 
        ! ----- Set residual
        if( fstrsolid%DLOAD_follow /= 0 .or. fstrsolid%CLOAD_ngrp_rot /= 0 ) &
          call fstr_ass_load(cstep, ctime+dtime, hecmesh, hecmat, fstrsolid, fstrparam)
 
        call fstr_update_ndforce(cstep,hecmesh,hecmat,fstrsolid,conmat )
 
        if( fstr_is_contact_active() )  then
          call hecmw_mat_clear_b( conmat )
          call fstr_update_ndforce_contact(cstep,hecmesh,hecmat,heclagmat,fstrsolid,conmat)
        endif
 
        res = fstr_get_norm_para_contact(hecmat,heclagmat,conmat,hecmesh)
 
        res = sqrt(res)/n_node_global
        if( iter == 1 ) res0 = res
        if( res0 == 0.0d0 ) then
          res0 =1.0d0
        else
          relres = dabs( res1-res )/res0
        endif
        if( hecmesh%my_rank == 0 ) then
          write(*, '(a,i3,a,2e15.7)') ' - Residual(',iter,') =',res,relres
        endif
 
        ! ----- check convergence
        if( res < fstrsolid%step_ctrl(cstep)%converg  .or.     &
          relres < fstrsolid%step_ctrl(cstep)%converg_ddisp ) exit
        res1 = res
 
        ! ----- check divergence and NaN
        if( iter == fstrsolid%step_ctrl(cstep)%max_iter .or. res > fstrsolid%step_ctrl(cstep)%maxres .or. res /= res ) then
          if( hecmesh%my_rank == 0) then
            write(   *,'(a,i5,a,i5)') '     ### Fail to Converge  : at total_step=', cstep, '  sub_step=', sub_step
          end if
          fstrsolid%NRstat_i(knstmaxit) = max(fstrsolid%NRstat_i(knstmaxit),iter) ! logging newton iteration(maxtier)
          fstrsolid%NRstat_i(knstsumit) = fstrsolid%NRstat_i(knstsumit) + iter    ! logging newton iteration(sumofiter)
          fstrsolid%NRstat_i(knstciter) = count_step                              ! logging contact iteration
          fstrsolid%CutBack_stat = fstrsolid%CutBack_stat + 1
          if( iter == fstrsolid%step_ctrl(cstep)%max_iter ) fstrsolid%NRstat_i(knstdresn) = 1
          if( res > fstrsolid%step_ctrl(cstep)%maxres .or. res /= res ) fstrsolid%NRstat_i(knstdresn) = 2
          return
        end if
 
      enddo
      ! ----- end of inner loop
 
      fstrsolid%NRstat_i(knstmaxit) = max(fstrsolid%NRstat_i(knstmaxit),iter) ! logging newton iteration(maxtier)
      fstrsolid%NRstat_i(knstsumit) = fstrsolid%NRstat_i(knstsumit) + iter    ! logging newton iteration(sum of iter)
 
      call fstr_scan_contact_state( cstep, sub_step, count_step, dtime, 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_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
 
      if( fstr_is_contact_conv(ctalgo,infoctchange,hecmesh) ) exit loopforcontactanalysis
 
      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
 
      ! ----- check divergence
      if( count_step >= fstrsolid%step_ctrl(cstep)%max_contiter ) then
        if( hecmesh%my_rank == 0) then
          write(   *,'(a,i5,a,i5)') '     ### Contact failed to Converge  : at total_step=', cstep, '  sub_step=', sub_step
        end if
        fstrsolid%NRstat_i(knstciter) = count_step                              ! logging contact iteration
        fstrsolid%CutBack_stat = fstrsolid%CutBack_stat + 1
        fstrsolid%NRstat_i(knstdresn) = 3
        return
      end if
 
      ! ----- Set residual for next newton iteration
      call fstr_update_ndforce(cstep,hecmesh,hecmat,fstrsolid,conmat )
 
      if( fstr_is_contact_active() )  then
        call hecmw_mat_clear_b( conmat )
        call fstr_update_ndforce_contact(cstep,hecmesh,hecmat,heclagmat,fstrsolid,conmat)
      endif
 
    enddo loopforcontactanalysis
 
    fstrsolid%NRstat_i(knstciter) = count_step ! logging contact iteration
 
    ! ----- update the total displacement
    !       u_{n+1} = u_{n} + \Delta u_{n+1}
    do i = 1, hecmesh%n_node*ndof
      fstrsolid%unode(i) = fstrsolid%unode(i)+fstrsolid%dunode(i)
    enddo
 
    call fstr_updatestate(hecmesh, fstrsolid, tincr)
    call fstr_update_contact_tangentforce( fstrsolid )
    if( fstrsolid%n_embeds > 0 .and. paracontactflag ) then
      call fstr_setup_parancon_contactvalue(hecmesh,ndof,fstrsolid%EMBED_NFORCE,1)
      call fstr_update_ndforce_spc( cstep, hecmesh, fstrsolid, hecmat%B )
    endif
 
    deallocate(coord)
    fstrsolid%CutBack_stat = 0
  end subroutine fstr_newton_contactslag
 
  subroutine fstr_init_newton(hecMESH, hecMAT, fstrSOLID, ctime, tincr, iter, cstep, dtime, fstrPARAM, hecLagMAT, ndof)
    use m_fstr_update
    implicit none
    type (hecmwST_local_mesh)             :: hecMESH
    type (hecmwST_matrix)                 :: hecMAT
    type (fstr_solid)                     :: fstrSOLID
    real(kind=kreal), intent(in)          :: ctime     
    real(kind=kreal), intent(in)          :: dtime     
    type (fstr_param)                     :: fstrPARAM
    real(kind=kreal), intent(inout)       :: tincr
    integer(kind=kint)                    :: iter
    integer, intent(in)                   :: cstep
    type (hecmwST_matrix_lagrange)        :: hecLagMAT
    integer(kind=kint), intent(inout)     :: ndof
 
    hecmat%NDOF = hecmesh%n_dof
    ndof = hecmat%ndof
 
    tincr = dtime
    if( fstrsolid%step_ctrl(cstep)%solution == stepstatic ) tincr = 0.d0
 
    fstrsolid%dunode(:) = 0.0d0
    fstrsolid%NRstat_i(:) = 0 ! logging newton iteration(init)
 
    call fstr_ass_load(cstep, ctime+dtime, hecmesh, hecmat, fstrsolid, fstrparam)
  end subroutine fstr_init_newton
 
  function fstr_check_iteration_converged(hecMESH, hecMAT, fstrSOLID, ndof, iter, sub_step, cstep) result(iterStatus)
    implicit none
    integer(kind=kint) :: iterstatus
 
    type (hecmwst_local_mesh)             :: hecmesh
    type (hecmwst_matrix)                 :: hecmat
    type (fstr_solid)                     :: fstrsolid
    integer(kind=kint), intent(in) :: ndof
    integer(kind=kint), intent(in) :: iter
    integer(kind=kint), intent(in) :: sub_step, cstep
 
    real(kind=kreal)   :: res, qnrm, rres, xnrm, dunrm, rxnrm
 
    iterstatus = kitrcontinue
 
    call hecmw_innerproduct_r(hecmesh, ndof, hecmat%B, hecmat%B, res)
    res = sqrt(res)
    call hecmw_innerproduct_r(hecmesh, ndof, hecmat%X, hecmat%X, xnrm)
    xnrm = sqrt(xnrm)
    call hecmw_innerproduct_r(hecmesh, ndof, fstrsolid%QFORCE, fstrsolid%QFORCE, qnrm)
    qnrm = sqrt(qnrm)
    if (qnrm < 1.0d-8) qnrm = 1.0d0
    if( iter == 1 ) then
      dunrm = xnrm
    else
      call hecmw_innerproduct_r(hecmesh, ndof, fstrsolid%dunode, fstrsolid%dunode, dunrm)
      dunrm = sqrt(dunrm)
    endif
    rres = res/qnrm
    rxnrm = xnrm/dunrm
 
    if( hecmesh%my_rank == 0 ) then
      if (qnrm == 1.0d0) then
        write(*,"(a,i8,a,1pe11.4,a,1pe11.4)")" iter:", iter, ", residual(abs):", rres, ", disp.corr.:", rxnrm
      else
        write(*,"(a,i8,a,1pe11.4,a,1pe11.4)")" iter:", iter, ", residual:", rres, ", disp.corr.:", rxnrm
      endif
    endif
    if( hecmw_mat_get_flag_diverged(hecmat) == kno ) then
      if( rres < fstrsolid%step_ctrl(cstep)%converg .or. &
          rxnrm < fstrsolid%step_ctrl(cstep)%converg_ddisp ) then
          iterstatus=kitrconverged
          return
      endif
    endif
 
    ! ----- check divergence and NaN
    if ( iter == fstrsolid%step_ctrl(cstep)%max_iter .or. rres > fstrsolid%step_ctrl(cstep)%maxres) &
      iterstatus = kitrdiverged
    if (rres /= rres ) iterstatus = kitrfloatingerror
    if (iterstatus /= kitrcontinue) then
      if( hecmesh%my_rank == 0) then
        write(ilog,'(a,i5,a,i5)') '### Fail to Converge  : at total_step=', cstep, '  sub_step=', sub_step
        write(   *,'(a,i5,a,i5)') '     ### Fail to Converge  : at total_step=', cstep, '  sub_step=', sub_step
      end if
      fstrsolid%NRstat_i(knstmaxit) = max(fstrsolid%NRstat_i(knstmaxit),iter) ! logging newton iteration(maxtier)
      fstrsolid%NRstat_i(knstsumit) = fstrsolid%NRstat_i(knstsumit) + iter    ! logging newton iteration(sumofiter)
      fstrsolid%CutBack_stat = fstrsolid%CutBack_stat + 1
      if( iter == fstrsolid%step_ctrl(cstep)%max_iter ) fstrsolid%NRstat_i(knstdresn) = 1
      if( rres > fstrsolid%step_ctrl(cstep)%maxres .or. rres /= rres ) fstrsolid%NRstat_i(knstdresn) = 2
      return
    end if
  end function fstr_check_iteration_converged
 
end module m_fstr_nonlinearmethod