1!-------------------------------------------------------------------------------
2! Copyright (c) 2019 FrontISTR Commons
3! This software is released under the MIT License, see LICENSE.txt
4!-------------------------------------------------------------------------------
5!> \brief This module provides functions:
6!!  1) obtain contact stiffness matrix of each contact pair and assemble
7!!     it into global stiffness matrix.
8!!  2) obtain contact nodal force vector of each contact pair and assemble
9!!      it into right-hand side vector to update non-equilibrated nodal force vector.
10!!  3) Modify Lagrange multiplier-related part of stiffness matrix and right-hand side
11!!     vector for dealing with prescribed displacement boundary condition.
12
13module m_addContactStiffness
14
15  use m_fstr
16  use elementInfo
17  use m_contact_lib
18  use fstr_matrix_con_contact
19  use hecmw_matrix_ass
20  use m_fstr_Residual
21
22  implicit none
23
24  public :: fstr_AddContactStiffness
25  public :: fstr_Update_NDForce_contact
26  public :: update_NDForce_contact
27  public :: fstr_ass_load_contact
28  public :: fstr_mat_ass_bc_contact
29
30  private :: getContactStiffness
31  private :: fstr_mat_ass_contact
32  private :: getContactNodalForce
33  private :: getTrialFricForceANDcheckFricState
34
35contains
36
37  !> \brief This subroutine obtains contact stiffness matrix of each contact pair
38  !!  and assembles it into global stiffness matrix.
39  subroutine fstr_AddContactStiffness(cstep,iter,hecMAT,fstrMAT,fstrSOLID)
40
41    integer(kind=kint)                   :: cstep !< current loding step
42    integer(kind=kint)                   :: iter
43    type(hecmwST_matrix)                 :: hecMAT !< type hecmwST_matrix
44    type(fstr_solid)                     :: fstrSOLID !< type fstr_solid
45    type(fstrST_matrix_contact_lagrange) :: fstrMAT !< type fstrST_matrix_contact_lagrange
46    integer(kind=kint)                   :: ctsurf, etype, nnode, ndLocal(9) !< contants of type tContact
47    integer(kind=kint)                   :: i, j, id_lagrange, grpid
48    real(kind=kreal)                     :: lagrange
49    real(kind=kreal)                     :: stiffness(9*3 + 1, 9*3 + 1)
50
51    fstrMAT%AL_lagrange = 0.0d0
52    fstrMAT%AU_lagrange = 0.0d0
53
54    id_lagrange = 0
55
56    do i = 1, size(fstrSOLID%contacts)
57
58      grpid = fstrSOLID%contacts(i)%group
59      if( .not. fstr_isContactActive( fstrSOLID, grpid, cstep ) ) cycle
60
61      do j = 1, size(fstrSOLID%contacts(i)%slave)
62
63        if( fstrSOLID%contacts(i)%states(j)%state == CONTACTFREE ) cycle
64
65        id_lagrange = id_lagrange + 1
66        lagrange = fstrMAT%Lagrange(id_lagrange)
67
68        ctsurf = fstrSOLID%contacts(i)%states(j)%surface
69        etype = fstrSOLID%contacts(i)%master(ctsurf)%etype
70        nnode = size(fstrSOLID%contacts(i)%master(ctsurf)%nodes)
71        ndLocal(1) = fstrSOLID%contacts(i)%slave(j)
72        ndLocal(2:nnode+1) = fstrSOLID%contacts(i)%master(ctsurf)%nodes(1:nnode)
73
74        ! Obtain contact stiffness matrix of contact pair
75        call getContactStiffness(iter,etype,nnode,fstrSOLID%contacts(i)%states(j),  &
76          fstrSOLID%contacts(i)%tPenalty,fstrSOLID%contacts(i)%fcoeff,lagrange,stiffness)
77
78        ! Assemble contact stiffness matrix of contact pair into global stiffness matrix
79        call fstr_mat_ass_contact(nnode,ndLocal,id_lagrange,fstrSOLID%contacts(i)%fcoeff,stiffness,hecMAT,fstrMAT)
80
81      enddo
82
83    enddo
84
85
86  end subroutine fstr_AddContactStiffness
87
88
89  !> \brief This subroutine obtains contact stiffness matrix of contact pair
90  subroutine getContactStiffness(iter,etype,nnode,ctState,tPenalty,fcoeff,lagrange,stiffness)
91
92    type(tContactState) :: ctState !< type tContactState
93    integer(kind=kint)  :: iter
94    integer(kind=kint)  :: etype, nnode !< type of master segment; number of nodes of master segment
95    integer(kind=kint)  :: i, j, k, l
96    real(kind=kreal)    :: normal(3), shapefunc(nnode) !< normal vector at target point; shape functions
97    real(kind=kreal)    :: nTm((nnode + 1)*3) !< vector
98    real(kind=kreal)    :: fcoeff, tPenalty !< friction coefficient; tangential penalty
99    real(kind=kreal)    :: lagrange !< value of Lagrange multiplier
100    real(kind=kreal)    :: tf_trial(3), length_tft
101    real(kind=kreal)    :: tangent(3), tTm((nnode + 1)*3)
102    real(kind=kreal)    :: stiffness(9*3 + 1, 9*3 + 1) !< contact stiffness matrix
103
104    stiffness = 0.0d0
105
106    call getShapeFunc( etype, ctState%lpos(:), shapefunc )
107
108    normal(1:3) = ctState%direction(1:3)
109
110    nTm(1:3) = normal(1:3)
111    do i = 1, nnode
112      nTm(i*3+1:i*3+3) = -shapefunc(i)*normal(1:3)
113    enddo
114
115    i = (nnode+1)*3 + 1
116    do j = 1, (nnode+1)*3
117      stiffness(i,j) = nTm(j);  stiffness(j,i) = nTm(j)
118    enddo
119
120
121    if( fcoeff /= 0.0d0 ) then
122      if( lagrange>0.0d0 .or. iter==1 ) then
123
124        do i = 1, nnode+1
125          do j = 1, i
126            do k = 1, 3
127              do l = 1, 3
128                stiffness((i-1)*3+k,(j-1)*3+l) = stiffness((i-1)*3+k,(j-1)*3+l) - tPenalty*nTm((i-1)*3+k)*nTm((j-1)*3+l)
129                if( k==l ) then
130                  if(i==1 .and. j==1)then
131                    stiffness((i-1)*3+k,(j-1)*3+l) = stiffness((i-1)*3+k,(j-1)*3+l) + tPenalty
132                  elseif(i>1 .and. j==1)then
133                    stiffness((i-1)*3+k,(j-1)*3+l) = stiffness((i-1)*3+k,(j-1)*3+l) - tPenalty*shapefunc(i-1)
134                  elseif(i>1 .and. j>1)then
135                    stiffness((i-1)*3+k,(j-1)*3+l) = stiffness((i-1)*3+k,(j-1)*3+l) + tPenalty*shapefunc(i-1)*shapefunc(j-1)
136                  endif
137                endif
138                if(i==j) cycle
139                stiffness((j-1)*3+l,(i-1)*3+k) = stiffness((i-1)*3+k,(j-1)*3+l)
140              enddo
141            enddo
142          enddo
143        enddo
144
145        if( ctstate%state == contactSlip ) then
146
147          tf_trial(1:3) = ctstate%tangentForce_trial(1:3)
148          length_tft = dsqrt(dot_product(tf_trial,tf_trial))
149          tangent(1:3) = tf_trial(1:3)/length_tft
150
151          tTm(1:3) = -tangent(1:3)
152          do i = 1, nnode
153            tTm(i*3+1:i*3+3) = shapefunc(i)*tangent(1:3)
154          enddo
155
156          do i = 1, nnode+1
157            do j = 1, nnode+1
158              do k = 1, 3
159                do l = 1, 3
160                  stiffness((i-1)*3+k,(j-1)*3+l) = stiffness((i-1)*3+k,(j-1)*3+l)       &
161                    + tPenalty*(-tTm((i-1)*3+k)*tTm((j-1)*3+l)  &
162                    +tTm((i-1)*3+k)*nTm((j-1)*3+l)*dot_product(tangent,normal))
163                enddo
164              enddo
165            enddo
166          enddo
167          stiffness(1:(nnode+1)*3,1:(nnode+1)*3) = (fcoeff*lagrange/length_tft)*stiffness(1:(nnode+1)*3,1:(nnode+1)*3)
168
169          !
170          !        do i = 1, (nnode + 1)*3
171          !          do j = 1, i
172          !            do k = 1, 3
173          !              do l = 1, k
174          !                stiffness((i-1)*3+k,(j-1)*3+l) = stiffness((i-1)*3+k,(j-1)*3+l) - mut*tTm((i-1)*3+k)*tTm((j-1)*3+l))
175          !                if( k/=l )stiffness((j-1)*3+l,(i-1)*3+k) = stiffness((i-1)*3+k,(j-1)*3+l)
176          !              enddo !- l
177          !            enddo !- k
178          !          enddo !- j
179          !        enddo !- i
180          !        stiffness(1:(nnode+1)*3,1:(nnode+1)*3) = (fcoeff*dabs(lagrange)/length_tft)*stiffness(1:(nnode+1)*3,1:(nnode+1)*3)
181
182          !         j = (nnode+1)*3 + 1
183          !         do i = 1, (nnode+1)*3
184          !           stiffness(i,j) = stiffness(i,j) - fcoeff*tTm(i)
185          !         enddo
186          stiffness(1:(nnode+1)*3,(nnode+1)*3+1) = stiffness(1:(nnode+1)*3,(nnode+1)*3+1) - fcoeff*tTm(1:(nnode+1)*3)
187
188        endif
189      endif
190    endif
191
192  end subroutine getContactStiffness
193
194
195  !> \brief This subroutine assembles contact stiffness matrix of a contact pair into global stiffness matrix
196  subroutine fstr_mat_ass_contact(nnode,ndLocal,id_lagrange,fcoeff,stiffness,hecMAT,fstrMAT)
197
198    type(hecmwST_matrix)                 :: hecMAT !< type hecmwST_matrix
199    type(fstrST_matrix_contact_lagrange) :: fstrMAT !< type fstrST_matrix_contact_lagrange
200    integer(kind=kint) :: nnode, ndLocal(nnode + 1), id_lagrange !< total number of nodes of master segment
201!< global number of nodes of contact pair
202!< number of Lagrange multiplier
203    integer(kind=kint) :: i, j, inod, jnod, l
204    integer(kind=kint) :: isL, ieL, idxL_base, kL, idxL, isU, ieU, idxU_base, kU, idxU
205    real(kind=kreal)   :: fcoeff !< friction coefficient
206    real(kind=kreal)   :: stiffness(9*3 + 1, 9*3 + 1) !< contact stiffness matrix
207    real(kind=kreal)   :: a(3, 3)
208
209    i = nnode + 1 + 1
210    inod = id_lagrange
211    isL = fstrMAT%indexL_lagrange(inod-1)+1
212    ieL = fstrMAT%indexL_lagrange(inod)
213
214    do j = 1, nnode + 1
215      jnod = ndLocal(j)
216      isU = fstrMAT%indexU_lagrange(jnod-1)+1
217      ieU = fstrMAT%indexU_lagrange(jnod)
218
219      kL = hecmw_array_search_i(fstrMAT%itemL_lagrange,isL,ieL,jnod)
220      if( kL<isL .or. kL>ieL ) then
221        write(*,*) '###ERROR### : cannot find connectivity (Lagrange1)'
222        stop
223      endif
224      kU = hecmw_array_search_i(fstrMAT%itemU_lagrange,isU,ieU,inod)
225      if( kU<isU .or. kU>ieU ) then
226        write(*,*) '###ERROR### : cannot find connectivity (Lagrange2)'
227        stop
228      endif
229
230      idxL_base = (kL-1)*3
231      idxU_base = (kU-1)*3
232
233      do l = 1, 3
234        idxL = idxL_base + l
235        fstrMAT%AL_lagrange(idxL) = fstrMAT%AL_lagrange(idxL) + stiffness((i-1)*3+1,(j-1)*3+l)
236        idxU = idxU_base + l
237        fstrMAT%AU_lagrange(idxU) = fstrMAT%AU_lagrange(idxU) + stiffness((j-1)*3+l,(i-1)*3+1)
238      enddo
239    enddo
240
241
242    if(fcoeff /= 0.0d0)then
243
244      do i = 1, nnode + 1
245        inod = ndLocal(i)
246        do j = 1, nnode + 1
247          jnod = ndLocal(j)
248          call stf_get_block(stiffness(1:(nnode+1)*3,1:(nnode+1)*3), 3, i, j, a)
249          call hecmw_mat_add_node(hecMAT, inod, jnod, a)
250        enddo
251      enddo
252
253    endif
254
255  end subroutine fstr_mat_ass_contact
256
257
258  !> \brief This subroutine obtains contact nodal force vector of each contact pair
259  !! and assembles it into right-hand side vector to update non-equilibrated nodal force vector.
260  subroutine fstr_Update_NDForce_contact(cstep,hecMESH,hecMAT,fstrMAT,fstrSOLID,conMAT)
261
262    type(hecmwST_local_mesh)             :: hecMESH !< type hecmwST_local_mesh
263    type(hecmwST_matrix)                 :: hecMAT !< type hecmwST_matrix
264    type(fstr_solid)                     :: fstrSOLID !< type fstr_solid
265    type(fstrST_matrix_contact_lagrange) :: fstrMAT !< type fstrST_matrix_contact_lagrange
266    type(hecmwST_matrix), optional       :: conMAT !< type hecmwST_matrix for contact part only
267    integer(kind=kint) :: ctsurf, etype, nnode, ndLocal(9) !< contants of type tContact
268    integer(kind=kint) :: i, j, k, id_lagrange
269    real(kind=kreal)   :: ndCoord(9*3), ndDu(9*3) !< nodal coordinates ; nodal displacement increment
270    real(kind=kreal)   :: lagrange !< value of Lagrange multiplier
271    real(kind=kreal)                        :: ctNForce(9*3+1)                           !< nodal normal contact force vector
272    real(kind=kreal)                        :: ctTForce(9*3+1)                           !< nodal tangential contact force vector
273
274    integer(kind=kint) :: cstep !< current calculation step
275    integer(kind=kint) :: grpid
276
277    id_lagrange = 0
278    if( associated(fstrSOLID%CONT_NFORCE) ) fstrSOLID%CONT_NFORCE(:) = 0.d0
279    if( associated(fstrSOLID%CONT_FRIC) ) fstrSOLID%CONT_FRIC(:) = 0.d0
280
281    do i = 1, size(fstrSOLID%contacts)
282
283      grpid = fstrSOLID%contacts(i)%group
284      if( .not. fstr_isContactActive( fstrSOLID, grpid, cstep ) ) cycle
285
286      do j = 1, size(fstrSOLID%contacts(i)%slave)
287
288        if( fstrSOLID%contacts(i)%states(j)%state == CONTACTFREE ) cycle
289
290        id_lagrange = id_lagrange + 1
291        lagrange = fstrMAT%Lagrange(id_lagrange)
292
293        fstrSOLID%contacts(i)%states(j)%multiplier(1) = fstrMAT%Lagrange(id_lagrange)
294
295        ctsurf = fstrSOLID%contacts(i)%states(j)%surface
296        etype = fstrSOLID%contacts(i)%master(ctsurf)%etype
297        nnode = size(fstrSOLID%contacts(i)%master(ctsurf)%nodes)
298        ndLocal(1) = fstrSOLID%contacts(i)%slave(j)
299        ndLocal(2:nnode+1) = fstrSOLID%contacts(i)%master(ctsurf)%nodes(1:nnode)
300        do k = 1, nnode+1
301          ndCoord((k-1)*3+1:(k-1)*3+3) = hecMESH%node((ndLocal(k)-1)*3+1:(ndLocal(k)-1)*3+3)          &
302            + fstrSOLID%unode((ndLocal(k)-1)*3+1:(ndLocal(k)-1)*3+3)       &
303            + fstrSOLID%dunode((ndLocal(k)-1)*3+1:(ndLocal(k)-1)*3+3)
304          ndDu((k-1)*3+1:(k-1)*3+3) = fstrSOLID%dunode((ndLocal(k)-1)*3+1:(ndLocal(k)-1)*3+3)
305        enddo
306        ! Obtain contact nodal force vector of contact pair
307        call getContactNodalForce(etype,nnode,ndCoord,ndDu,fstrSOLID%contacts(i)%states(j),    &
308          fstrSOLID%contacts(i)%tPenalty,fstrSOLID%contacts(i)%fcoeff,lagrange,ctNForce,ctTForce)
309        ! Update non-eqilibrited force vector
310        if(present(conMAT)) then
311          call update_NDForce_contact(nnode,ndLocal,id_lagrange,lagrange,ctNForce,ctTForce,fstrSOLID,conMAT)
312        else
313          call update_NDForce_contact(nnode,ndLocal,id_lagrange,lagrange,ctNForce,ctTForce,fstrSOLID,hecMAT)
314        endif
315
316      enddo
317
318    enddo
319
320    !    Consider SPC condition
321    call fstr_Update_NDForce_SPC(cstep, hecMESH, fstrSOLID, hecMAT%B)
322    if(present(conMAT)) call fstr_Update_NDForce_SPC(cstep, hecMESH, fstrSOLID, conMAT%B)
323
324  end subroutine fstr_Update_NDForce_contact
325
326  !> \brief This subroutine obtains contact nodal force vector of contact pair
327  subroutine getContactNodalForce(etype,nnode,ndCoord,ndDu,ctState,tPenalty,fcoeff,lagrange,ctNForce,ctTForce)
328
329    type(tContactState) :: ctState !< type tContactState
330    integer(kind=kint) :: etype, nnode !< type of master segment; number of nodes of master segment
331    integer(kind=kint) :: i, j
332    real(kind=kreal)   :: fcoeff, tPenalty !< friction coefficient; tangential penalty
333    real(kind=kreal)   :: lagrange !< value of Lagrange multiplier
334    real(kind=kreal)   :: ndCoord((nnode + 1)*3), ndDu((nnode + 1)*3) !< nodal coordinates; nodal displacement increment
335    real(kind=kreal)   :: normal(3), shapefunc(nnode) !< normal vector at target point; shape functions
336    real(kind=kreal)   :: nTm((nnode + 1)*3) !< vector
337    real(kind=kreal)   :: tf_trial(3), length_tft, tangent(3), tf_final(3)
338    real(kind=kreal)       :: ctNForce((nnode+1)*3+1)                     !< contact force vector
339    real(kind=kreal)       :: ctTForce((nnode+1)*3+1)                     !< contact force vector
340
341    ctNForce = 0.0d0
342    ctTForce = 0.0d0
343
344    call getShapeFunc( etype, ctState%lpos(:), shapefunc )
345
346    normal(1:3) = ctState%direction(1:3)
347
348    nTm(1:3) = -normal(1:3)
349    do i = 1, nnode
350      nTm(i*3+1:i*3+3) = shapefunc(i)*normal(1:3)
351    enddo
352
353    do j = 1, (nnode+1)*3
354      ctNForce(j) = lagrange*nTm(j)
355    enddo
356    j = (nnode+1)*3 + 1
357    ctNForce(j) = dot_product(nTm,ndCoord)
358
359    if(fcoeff /= 0.0d0 .and. lagrange > 0.0d0)then
360
361      call getTrialFricForceANDcheckFricState(nnode,tPenalty,fcoeff,lagrange,normal,shapefunc,nTm,ndDu,ctstate)
362
363      if( ctstate%state == contactStick ) then
364        tf_final(1:3) = ctstate%tangentForce_trial(1:3)
365      elseif( ctstate%state == contactSlip ) then
366        tf_trial(1:3) = ctstate%tangentForce_trial(1:3)
367        length_tft = dsqrt(dot_product(tf_trial,tf_trial))
368        tangent(1:3) = tf_trial(1:3)/length_tft
369        tf_final(1:3) = fcoeff*dabs(lagrange)*tangent(1:3)
370      endif
371
372      ctTForce(1:3) = -tf_final(1:3)
373      do j = 1, nnode
374        ctTForce(j*3+1:j*3+3) = shapefunc(j)*tf_final(1:3)
375      enddo
376
377      ctstate%tangentForce_final(1:3) = tf_final(1:3)
378
379    endif
380
381  end subroutine getContactNodalForce
382
383
384  !> \brief This subroutine calculates trial friction force and checks friction state
385  subroutine getTrialFricForceANDcheckFricState(nnode,tPenalty,fcoeff,lagrange,normal,shapefunc,nTm,ndDu,ctstate)
386
387    type(tContactState) :: ctState !< type tContactState
388    integer(kind=kint) :: nnode !< number of nodes of master segment
389    integer(kind=kint) :: i, j
390    real(kind=kreal)   :: fcoeff, tPenalty !< friction coefficient; tangential penalty
391    real(kind=kreal)   :: lagrange !< value of Lagrange multiplier
392    real(kind=kreal)   :: ndDu((nnode + 1)*3) !< nodal displacement increment
393    real(kind=kreal)   :: normal(3), shapefunc(nnode) !< normal vector at target point; shape functions
394    real(kind=kreal)   :: nTm((nnode + 1)*3) !< vector
395    real(kind=kreal)   :: dotP
396    real(kind=kreal)   :: relativeDisp(3) !< relative displacement
397    real(kind=kreal)   :: tf_yield
398
399    relativeDisp = 0.0d0
400
401    dotP = dot_product(nTm,ndDu)
402    do i = 1, 3
403      relativeDisp(i) = - ndDu(i)
404      do j = 1, nnode
405        relativeDisp(i) = relativeDisp(i) + shapefunc(j)*ndDu(j*3+i)
406      enddo
407      relativeDisp(i) = relativeDisp(i) - dotP*normal(i)
408      ctstate%reldisp(i) = -relativeDisp(i)
409      ctstate%tangentForce_trial(i) = ctstate%tangentForce1(i) -tPenalty*relativeDisp(i)
410    enddo
411
412    tf_yield = fcoeff*dabs(lagrange)
413    if(ctstate%state == contactSlip) tf_yield =0.99d0*tf_yield
414    if( dsqrt(dot_product(ctstate%tangentForce_trial,ctstate%tangentForce_trial)) <= tf_yield ) then
415      ctstate%state = contactStick
416    else
417      ctstate%state = contactSlip
418    endif
419
420  end subroutine getTrialFricForceANDcheckFricState
421
422
423  !> \brief This subroutine assembles contact nodal force vector into right-hand side vector
424  !! to update non-equilibrated nodal force vector.
425  subroutine update_NDForce_contact(nnode,ndLocal,id_lagrange,lagrange,ctNForce,ctTForce,fstrSOLID,hecMAT)
426
427    type(fstr_solid)                        :: fstrSOLID                       !< type fstr_solid
428    type(hecmwST_matrix)                 :: hecMAT !< type hecmwST_matrix
429    integer(kind=kint) :: nnode, ndLocal(nnode + 1) !< number of nodes of master segment
430    !< global number of nodes of contact pair
431    integer(kind=kint) :: id_lagrange !< number of Lagrange multiplier
432    real(kind=kreal)                        :: lagrange                        !< value of Lagrange multiplier
433    integer(kind=kint) :: np, ndof !< total number of nodes; degree of freedom
434    integer (kind=kint)                     :: i, inod, idx
435    real(kind=kreal)                        :: ctNForce((nnode+1)*3+1)         !< contact force vector
436    real(kind=kreal)                        :: ctTForce((nnode+1)*3+1)         !< contact force vector
437
438    np = hecMAT%NP; ndof = hecMAT%NDOF
439
440    do i = 1, nnode + 1
441      inod = ndLocal(i)
442      idx = (inod-1)*3+1
443      hecMAT%B(idx:idx+2) = hecMAT%B(idx:idx+2) + ctNForce((i-1)*3+1:(i-1)*3+3) + ctTForce((i-1)*3+1:(i-1)*3+3)
444      if( lagrange < 0.d0 ) cycle
445      fstrSOLID%CONT_NFORCE(idx:idx+2) = fstrSOLID%CONT_NFORCE(idx:idx+2) + ctNForce((i-1)*3+1:(i-1)*3+3)
446      fstrSOLID%CONT_FRIC(idx:idx+2) = fstrSOLID%CONT_FRIC(idx:idx+2) + ctTForce((i-1)*3+1:(i-1)*3+3)
447    enddo
448
449    hecMAT%B(np*ndof+id_lagrange) = ctNForce((nnode+1)*3+1)+ctTForce((nnode+1)*3+1)
450
451  end subroutine update_NDForce_contact
452
453
454  !> \brief This subroutine adds initial contact force vector to the right-hand side vector
455  !> \at the beginning of each substep calculation
456  subroutine fstr_ass_load_contact(cstep, hecMESH, hecMAT, fstrSOLID, fstrMAT)
457
458    type(hecmwST_local_mesh)             :: hecMESH !< type hecmwST_local_mesh
459    type(hecmwST_matrix)                 :: hecMAT !< type hecmwST_matrix
460    type(fstr_solid)                     :: fstrSOLID !< type fstr_solid
461    type(fstrST_matrix_contact_lagrange) :: fstrMAT !< type fstrST_matrix_contact_lagrange
462    integer(kind=kint) :: cstep !< current step
463    integer(kind=kint) :: np, ndof !< total number of nodes; degree of freedom
464    integer(kind=kint) :: i, j, k, l, id_lagrange, lnod, grpid
465    integer(kind=kint) :: ctsurf, etype, nnode, ndLocal(9) !< contants of type tContact
466    real(kind=kreal)   :: ndCoord(9*3), lagrange !< nodal coordinates; value of Lagrange mutiplier
467    real(kind=kreal)   :: normal(3), shapefunc(9) !< normal vector; shape functions
468    real(kind=kreal)   :: nTm(10*3) !< vector
469    real(kind=kreal)   :: tf_final(3) !< final friciton force vector
470    real(kind=kreal)   :: ctForce(9*3 + 1) !< initial nodal contact force vector
471
472    np = hecMAT%NP ; ndof = hecMAT%NDOF
473
474    id_lagrange = 0
475
476    do i = 1, size(fstrSOLID%contacts)
477
478      grpid = fstrSOLID%contacts(i)%group
479      if( .not. fstr_isContactActive( fstrSOLID, grpid, cstep ) ) cycle
480
481      do j = 1, size(fstrSOLID%contacts(i)%slave)
482
483        if( fstrSOLID%contacts(i)%states(j)%state == CONTACTFREE ) cycle
484
485        id_lagrange = id_lagrange + 1
486        lagrange = fstrSOLID%contacts(i)%states(j)%multiplier(1)
487
488        ctsurf = fstrSOLID%contacts(i)%states(j)%surface
489        etype = fstrSOLID%contacts(i)%master(ctsurf)%etype
490        nnode = size(fstrSOLID%contacts(i)%master(ctsurf)%nodes)
491        ndLocal(1) = fstrSOLID%contacts(i)%slave(j)
492        ndLocal(2:nnode+1) = fstrSOLID%contacts(i)%master(ctsurf)%nodes(1:nnode)
493        do k = 1, nnode+1
494          ndCoord((k-1)*3+1:(k-1)*3+3) = hecMESH%node((ndLocal(k)-1)*3+1:(ndLocal(k)-1)*3+3)          &
495            + fstrSOLID%unode((ndLocal(k)-1)*3+1:(ndLocal(k)-1)*3+3)
496        enddo
497
498        ctForce = 0.0d0
499
500        call getShapeFunc( etype, fstrSOLID%contacts(i)%states(j)%lpos(:), shapefunc )
501        normal(1:3) = fstrSOLID%contacts(i)%states(j)%direction(1:3)
502        nTm(1:3) = -normal(1:3)
503        do k = 1, nnode
504          nTm(k*3+1:k*3+3) = shapefunc(k)*normal(1:3)
505        enddo
506        do l = 1, (nnode+1)*3
507          ctForce(l) = lagrange*nTm(l)
508        enddo
509        l = (nnode+1)*3 + 1
510        ctForce(l) = dot_product(nTm(1:(nnode+1)*3),ndCoord(1:(nnode+1)*3))
511
512        if( fstrSOLID%contacts(i)%fcoeff/=0.0d0 .and. lagrange>0.0d0 )then
513          tf_final(1:3) = fstrSOLID%contacts(i)%states(j)%tangentForce_final(1:3)
514          ctForce(1:3) = ctForce(1:3) - tf_final(1:3)
515          do l = 1, nnode
516            ctForce(l*3+1:l*3+3) = ctForce(l*3+1:l*3+3) + shapefunc(l)*tf_final(1:3)
517          enddo
518        endif
519
520        do l = 1, nnode + 1
521          lnod = ndLocal(l)
522          hecMAT%B((lnod-1)*3+1:(lnod-1)*3+3) = hecMAT%B((lnod-1)*3+1:(lnod-1)*3+3) + ctForce((l-1)*3+1:(l-1)*3+3)
523        enddo
524        hecMAT%B(np*ndof+id_lagrange) = ctForce((nnode+1)*3+1)
525
526      enddo
527
528    enddo
529
530  end subroutine fstr_ass_load_contact
531
532
533  !> Modify Lagrange multiplier-related part of stiffness matrix and right-hand side vector
534  !> for dealing with prescribed displacement boundary condition
535  subroutine fstr_mat_ass_bc_contact(hecMAT,fstrMAT,inode,idof,RHS)
536
537    type(hecmwST_matrix)                 :: hecMAT !< hecmwST_matrix
538    type(fstrST_matrix_contact_lagrange) :: fstrMAT !< fstrST_matrix_contact_lagrange
539    integer(kind=kint) :: inode, idof !< number of node; degree of freedom
540    integer(kind=kint) :: isU, ieU, isL, ieL, i, l, k
541    real(kind=kreal)   :: RHS !< value of prescribed displacement
542
543    isU = fstrMAT%indexU_lagrange(inode-1)+1
544    ieU = fstrMAT%indexU_lagrange(inode)
545    do i = isU, ieU
546      fstrMAT%AU_lagrange((i-1)*3+idof) = 0.0d0
547      l = fstrMAT%itemU_lagrange(i)
548      isL = fstrMAT%indexL_lagrange(l-1)+1
549      ieL = fstrMAT%indexL_lagrange(l)
550      k = hecmw_array_search_i(fstrMAT%itemL_lagrange,isL,ieL,inode)
551      if(k < isL .or. k > ieL) cycle
552      hecMAT%B(hecMAT%NP*hecMAT%NDOF+l) = hecMAT%B(hecMAT%NP*hecMAT%NDOF+l) - fstrMAT%AL_lagrange((k-1)*3+idof)*RHS
553      fstrMAT%AL_lagrange((k-1)*3+idof) = 0.0d0
554    enddo
555
556  end subroutine fstr_mat_ass_bc_contact
557
558end module m_addContactStiffness
559
560
561
562