1!-------------------------------------------------------------------------------
2! Copyright (c) 2019 FrontISTR Commons
3! This software is released under the MIT License, see LICENSE.txt
4!-------------------------------------------------------------------------------
5
6module hecmw_matrix_ass
7  use hecmw_util
8  use m_hecmw_comm_f
9  use hecmw_matrix_misc
10  use hecmw_matrix_contact
11  implicit none
12
13  private
14
15  public :: hecmw_mat_ass_elem
16  public :: hecmw_mat_add_node
17  public :: hecmw_array_search_i
18  public :: hecmw_mat_ass_equation
19  public :: hecmw_mat_ass_equation_rhs
20  public :: hecmw_mat_add_dof
21  public :: hecmw_mat_ass_bc
22  public :: hecmw_mat_ass_contact
23  public :: stf_get_block
24
25contains
26
27  !C
28  !C***
29  !C*** MAT_ASS_ELEM
30  !C***
31  !C
32  subroutine hecmw_mat_ass_elem(hecMAT, nn, nodLOCAL, stiffness)
33    type (hecmwST_matrix)     :: hecMAT
34    integer(kind=kint) :: nn
35    integer(kind=kint) :: nodLOCAL(:)
36    real(kind=kreal) :: stiffness(:, :)
37    !** Local variables
38    integer(kind=kint) :: ndof, inod_e, jnod_e, inod, jnod
39    real(kind=kreal) :: a(6,6)
40
41    ndof = hecMAT%NDOF
42
43    do inod_e = 1, nn
44      inod = nodLOCAL(inod_e)
45      do jnod_e = 1, nn
46        jnod = nodLOCAL(jnod_e)
47        !***** Add components
48        call stf_get_block(stiffness, ndof, inod_e, jnod_e, a)
49        call hecmw_mat_add_node(hecMAT, inod, jnod, a)
50      enddo
51    enddo
52
53  end subroutine hecmw_mat_ass_elem
54
55  subroutine stf_get_block(stiffness, ndof, inod, jnod, a)
56    real(kind=kreal) :: stiffness(:, :), a(:, :)
57    integer(kind=kint) :: ndof, inod, jnod
58    !** Local variables
59    integer(kind=kint) :: row_offset, col_offset, i, j
60
61    row_offset = ndof*(inod-1)
62    do i = 1, ndof
63
64      col_offset = ndof*(jnod-1)
65      do j = 1, ndof
66
67        a(i, j) = stiffness(i + row_offset, j + col_offset)
68      enddo
69    enddo
70  end subroutine stf_get_block
71
72
73  subroutine hecmw_mat_add_node(hecMAT, inod, jnod, a)
74    type (hecmwST_matrix) :: hecMAT
75    integer(kind=kint) :: inod, jnod
76    real(kind=kreal) :: a(:, :)
77    !** Local variables
78    integer(kind=kint) :: NDOF, is, iE, k, idx_base, idx, idof, jdof
79
80    NDOF = hecMAT%NDOF
81
82    if (inod < jnod) then
83      is = hecMAT%indexU(inod-1)+1
84      iE = hecMAT%indexU(inod)
85      k = hecmw_array_search_i(hecMAT%itemU, is, iE, jnod)
86
87      if (k < is .or. iE < k) then
88        write(*,*) '###ERROR### : cannot find connectivity (1)'
89        write(*,*) ' myrank = ', hecmw_comm_get_rank(), ', inod = ', inod, ', jnod = ', jnod
90        call hecmw_abort(hecmw_comm_get_comm())
91      endif
92
93      idx_base = NDOF**2 * (k-1)
94      do idof = 1, NDOF
95        do jdof = 1, NDOF
96          idx = idx_base + jdof
97          !$omp atomic
98          hecMAT%AU(idx) = hecMAT%AU(idx) + a(idof, jdof)
99        enddo
100        idx_base = idx_base + NDOF
101      enddo
102
103    else if (inod > jnod) then
104      is = hecMAT%indexL(inod-1)+1
105      iE = hecMAT%indexL(inod)
106      k = hecmw_array_search_i(hecMAT%itemL, is, iE, jnod)
107
108      if (k < is .or. iE < k) then
109        write(*,*) '###ERROR### : cannot find connectivity (2)'
110        write(*,*) ' myrank = ', hecmw_comm_get_rank(), ', inod = ', inod, ', jnod = ', jnod
111        call hecmw_abort(hecmw_comm_get_comm())
112      endif
113
114      idx_base = NDOF**2 * (k-1)
115      do idof = 1, NDOF
116        do jdof = 1, NDOF
117          idx = idx_base + jdof
118          !$omp atomic
119          hecMAT%AL(idx) = hecMAT%AL(idx) + a(idof, jdof)
120        enddo
121        idx_base = idx_base + NDOF
122      enddo
123
124    else
125      idx_base = NDOF**2 * (inod - 1)
126      do idof = 1, NDOF
127        do jdof = 1, NDOF
128          idx = idx_base + jdof
129          !$omp atomic
130          hecMAT%D(idx) = hecMAT%D(idx) + a(idof, jdof)
131        enddo
132        idx_base = idx_base + NDOF
133      enddo
134    endif
135  end subroutine hecmw_mat_add_node
136
137
138  function hecmw_array_search_i(array, is, iE, ival)
139    integer(kind=kint) :: hecmw_array_search_i
140    integer(kind=kint) :: array(:)
141    integer(kind=kint) :: is, iE, ival
142    !** Local variables
143    integer(kind=kint) :: left, right, center, cval
144
145    left = is
146    right = iE
147    do
148      if (left > right) then
149        center = -1
150        exit
151      endif
152
153      center = (left + right) / 2
154      cval = array(center)
155
156      if (ival < cval) then
157        right = center - 1
158      else if (cval < ival) then
159        left = center + 1
160      else
161        exit
162      endif
163    enddo
164
165    hecmw_array_search_i = center
166
167  end function hecmw_array_search_i
168
169
170  !C
171  !C***
172  !C*** MAT_ASS_EQUATION
173  !C***
174  !C
175  subroutine hecmw_mat_ass_equation ( hecMESH, hecMAT )
176    type (hecmwST_matrix), target :: hecMAT
177    type (hecmwST_local_mesh)     :: hecMESH
178    !** Local variables
179    real(kind=kreal), pointer :: penalty
180    real(kind=kreal) :: ALPHA, a1_2inv, ai, aj, factor
181    integer(kind=kint) :: impc, is, iE, i, j, inod, idof, jnod, jdof
182    logical :: is_internal_i, is_internal_j
183
184    if( hecmw_mat_get_penalized(hecMAT) == 1 ) return
185
186    ! write(*,*) "INFO: imposing MPC by penalty"
187
188    penalty => hecMAT%Rarray(11)
189
190    if (penalty < 0.0) stop "ERROR: negative penalty"
191    if (penalty < 1.0) write(*,*) "WARNING: penalty ", penalty, " smaller than 1"
192
193    ALPHA= hecmw_mat_diag_max(hecMAT, hecMESH) * penalty
194    call hecmw_mat_set_penalty_alpha(hecMAT, ALPHA)
195
196    OUTER: do impc = 1, hecMESH%mpc%n_mpc
197      is = hecMESH%mpc%mpc_index(impc-1) + 1
198      iE = hecMESH%mpc%mpc_index(impc)
199
200      do i = is, iE
201        if (hecMESH%mpc%mpc_dof(i) > hecMAT%NDOF) cycle OUTER
202      enddo
203
204      a1_2inv = 1.0 / hecMESH%mpc%mpc_val(is)**2
205
206
207      do i = is, iE
208        inod = hecMESH%mpc%mpc_item(i)
209
210        is_internal_i = (hecMESH%node_ID(2*inod) == hecmw_comm_get_rank())
211
212        idof = hecMESH%mpc%mpc_dof(i)
213        ai = hecMESH%mpc%mpc_val(i)
214        factor = ai * a1_2inv
215
216        do j = is, iE
217          jnod = hecMESH%mpc%mpc_item(j)
218
219          is_internal_j = (hecMESH%node_ID(2*jnod) == hecmw_comm_get_rank())
220          if (.not. (is_internal_i .or. is_internal_j)) cycle
221
222          jdof = hecMESH%mpc%mpc_dof(j)
223          aj = hecMESH%mpc%mpc_val(j)
224
225          call hecmw_mat_add_dof(hecMAT, inod, idof, jnod, jdof, aj*factor*ALPHA)
226        enddo
227
228      enddo
229    enddo OUTER
230
231    call hecmw_mat_set_penalized(hecMAT, 1)
232
233  end subroutine hecmw_mat_ass_equation
234
235
236  subroutine hecmw_mat_ass_equation_rhs ( hecMESH, hecMAT )
237    type (hecmwST_matrix), target :: hecMAT
238    type (hecmwST_local_mesh)     :: hecMESH
239    !** Local variables
240    real(kind=kreal) :: ALPHA, a1_2inv, ai, factor, ci
241    integer(kind=kint) :: ndof, impc, iS, iE, i, inod, idof
242
243    if( hecmw_mat_get_penalized_b(hecMAT) == 1) return
244
245    ALPHA = hecmw_mat_get_penalty_alpha(hecMAT)
246    if (ALPHA <= 0.0) stop "ERROR: penalty applied on vector before matrix"
247
248    ndof = hecMAT%NDOF
249
250    OUTER: do impc = 1, hecMESH%mpc%n_mpc
251      iS = hecMESH%mpc%mpc_index(impc-1) + 1
252      iE = hecMESH%mpc%mpc_index(impc)
253
254      do i = is, iE
255        if (hecMESH%mpc%mpc_dof(i) > ndof) cycle OUTER
256      enddo
257
258      a1_2inv = 1.0 / hecMESH%mpc%mpc_val(iS)**2
259
260      do i = iS, iE
261        inod = hecMESH%mpc%mpc_item(i)
262
263        idof = hecMESH%mpc%mpc_dof(i)
264        ai = hecMESH%mpc%mpc_val(i)
265        factor = ai * a1_2inv
266
267        ci = hecMESH%mpc%mpc_const(impc)
268        !$omp atomic
269        hecMAT%B(ndof*(inod-1)+idof) = hecMAT%B(ndof*(inod-1)+idof) + ci*factor*ALPHA
270      enddo
271    enddo OUTER
272
273    call hecmw_mat_set_penalized_b(hecMAT, 1)
274
275  end subroutine hecmw_mat_ass_equation_rhs
276
277
278  subroutine hecmw_mat_add_dof(hecMAT, inod, idof, jnod, jdof, val)
279    type (hecmwST_matrix) :: hecMAT
280    integer(kind=kint) :: inod, idof, jnod, jdof
281    real(kind=kreal) :: val
282    !** Local variables
283    integer(kind=kint) :: NDOF, is, iE, k, idx
284
285    NDOF = hecMAT%NDOF
286    if (inod < jnod) then
287      is = hecMAT%indexU(inod-1)+1
288      iE = hecMAT%indexU(inod)
289      k = hecmw_array_search_i(hecMAT%itemU, is, iE, jnod)
290
291      if (k < is .or. iE < k) then
292        write(*,*) '###ERROR### : cannot find connectivity (3)'
293        write(*,*) '  myrank = ', hecmw_comm_get_rank(), ', inod = ', inod, ', jnod = ', jnod
294        call hecmw_abort(hecmw_comm_get_comm())
295        return
296      endif
297
298      idx = NDOF**2 * (k-1) + NDOF * (idof-1) + jdof
299      !$omp atomic
300      hecMAT%AU(idx) = hecMAT%AU(idx) + val
301
302    else if (inod > jnod) then
303      is = hecMAT%indexL(inod-1)+1
304      iE = hecMAT%indexL(inod)
305      k = hecmw_array_search_i(hecMAT%itemL, is, iE, jnod)
306
307      if (k < is .or. iE < k) then
308        write(*,*) '###ERROR### : cannot find connectivity (4)'
309        write(*,*) ' myrank = ', hecmw_comm_get_rank(), ', inod = ', inod, ', jnod = ', jnod
310        call hecmw_abort(hecmw_comm_get_comm())
311        return
312      endif
313
314      idx = NDOF**2 * (k-1) + NDOF * (idof-1) + jdof
315      !$omp atomic
316      hecMAT%AL(idx) = hecMAT%AL(idx) + val
317
318    else
319      idx = NDOF**2 * (inod - 1) + NDOF * (idof - 1) + jdof
320      !$omp atomic
321      hecMAT%D(idx) = hecMAT%D(idx) + val
322    endif
323
324  end subroutine hecmw_mat_add_dof
325
326  !C
327  !C***
328  !C*** MAT_ASS_BC
329  !C***
330  !C
331  subroutine hecmw_mat_ass_bc(hecMAT, inode, idof, RHS, conMAT)
332    type (hecmwST_matrix)     :: hecMAT
333    integer(kind=kint) :: inode, idof
334    real(kind=kreal) :: RHS, val
335    type (hecmwST_matrix),optional     :: conMAT
336    integer(kind=kint) :: NDOF, in, i, ii, iii, ndof2, k, is, iE, iiS, iiE, ik, idx
337
338    NDOF = hecMAT%NDOF
339    if( NDOF < idof ) return
340
341    !C-- DIAGONAL block
342
343    hecMAT%B(NDOF*inode-(NDOF-idof)) = RHS
344    if(present(conMAT)) conMAT%B(NDOF*inode-(NDOF-idof)) = 0.0D0
345    ndof2 = NDOF*NDOF
346    ii  = ndof2 - idof
347
348    do i = NDOF-1,0,-1
349      if( i .NE. NDOF-idof ) then
350        idx = NDOF*inode-i
351        val = hecMAT%D(ndof2*inode-ii)*RHS
352        !$omp atomic
353        hecMAT%B(idx) = hecMAT%B(idx) - val
354        if(present(conMAT)) then
355          val = conMAT%D(ndof2*inode-ii)*RHS
356          !$omp atomic
357          conMAT%B(idx) = conMAT%B(idx) - val
358        endif
359      endif
360      ii = ii - NDOF
361    end do
362
363    !*Set diagonal row to zero
364    ii  = ndof2-1 - (idof-1)*NDOF
365
366    do i = 0, NDOF - 1
367      hecMAT%D(ndof2*inode-ii+i)=0.d0
368      if(present(conMAT)) conMAT%D(ndof2*inode-ii+i)=0.d0
369    end do
370
371    !*Set diagonal column to zero
372    ii = ndof2 - idof
373    do i = 1, NDOF
374      if( i.NE.idof ) then
375        hecMAT%D(ndof2*inode-ii) = 0.d0
376        if(present(conMAT)) conMAT%D(ndof2*inode-ii) = 0.d0
377      else
378        hecMAT%D(ndof2*inode-ii) = 1.d0
379        if(present(conMAT)) conMAT%D(ndof2*inode-ii) = 0.d0
380      endif
381      ii = ii - NDOF
382    end do
383
384    !C-- OFF-DIAGONAL blocks
385
386    ii  = ndof2-1 - (idof-1)*NDOF
387    is = hecMAT%indexL(inode-1) + 1
388    iE = hecMAT%indexL(inode  )
389
390    do k= is, iE
391
392      !*row (left)
393      do i = 0, NDOF - 1
394        hecMAT%AL(ndof2*k-ii+i) = 0.d0
395        if(present(conMAT)) conMAT%AL(ndof2*k-ii+i) = 0.d0
396      end do
397
398      !*column (upper)
399      in = hecMAT%itemL(k)
400      iiS = hecMAT%indexU(in-1) + 1
401      iiE = hecMAT%indexU(in  )
402      do ik = iiS, iiE
403        if (hecMAT%itemU(ik) .eq. inode) then
404          iii = ndof2 - idof
405          do i = NDOF-1,0,-1
406            idx = NDOF*in-i
407            val = hecMAT%AU(ndof2*ik-iii)*RHS
408            !$omp atomic
409            hecMAT%B(idx) = hecMAT%B(idx) - val
410            hecMAT%AU(ndof2*ik-iii)= 0.d0
411            if(present(conMAT)) then
412              val = conMAT%AU(ndof2*ik-iii)*RHS
413              !$omp atomic
414              conMAT%B(idx) = conMAT%B(idx) - val
415              conMAT%AU(ndof2*ik-iii)= 0.d0
416            endif
417            iii = iii - NDOF
418          end do
419          exit
420        endif
421      enddo
422
423    enddo
424
425    ii = ndof2-1 - (idof-1)*NDOF
426    is = hecMAT%indexU(inode-1) + 1
427    iE = hecMAT%indexU(inode  )
428
429    do k= is, iE
430
431      !*row (right)
432      do i = 0,NDOF-1
433        hecMAT%AU(ndof2*k-ii+i) = 0.d0
434        if(present(conMAT)) conMAT%AU(ndof2*k-ii+i) = 0.d0
435      end do
436
437      !*column (lower)
438      in = hecMAT%itemU(k)
439      iiS = hecMAT%indexL(in-1) + 1
440      iiE = hecMAT%indexL(in  )
441      do ik= iiS, iiE
442        if (hecMAT%itemL(ik) .eq. inode) then
443          iii  = ndof2 - idof
444
445          do i = NDOF-1, 0, -1
446            idx = NDOF*in-i
447            val = hecMAT%AL(ndof2*ik-iii)*RHS
448            !$omp atomic
449            hecMAT%B(idx) = hecMAT%B(idx) - val
450            hecMAT%AL(ndof2*ik-iii) = 0.d0
451            if(present(conMAT)) then
452              val = conMAT%AL(ndof2*ik-iii)*RHS
453              !$omp atomic
454              conMAT%B(idx) = conMAT%B(idx) - val
455              conMAT%AL(ndof2*ik-iii) = 0.d0
456            endif
457            iii = iii - NDOF
458          end do
459          exit
460        endif
461      enddo
462
463    enddo
464    !*End off - diagonal blocks
465
466    call hecmw_cmat_ass_bc(hecMAT, inode, idof, RHS)
467
468  end subroutine hecmw_mat_ass_bc
469
470  !C
471  !C***
472  !C*** MAT_ASS_CONTACT
473  !C***
474  !C
475  subroutine hecmw_mat_ass_contact(hecMAT, nn, nodLOCAL, stiffness)
476    type (hecmwST_matrix)     :: hecMAT
477    integer(kind=kint) :: nn
478    integer(kind=kint) :: nodLOCAL(:)
479    real(kind=kreal) :: stiffness(:, :)
480    !** Local variables
481    integer(kind=kint) :: ndof, inod_e, jnod_e, inod, jnod
482    real(kind=kreal) :: a(3,3)
483
484    ndof = hecMAT%NDOF
485    if( ndof .ne. 3 ) then
486      write(*,*) '###ERROR### : ndof=',ndof,'; contact matrix supports only ndof==3'
487      call hecmw_abort(hecmw_comm_get_comm())
488      return
489    endif
490
491    do inod_e = 1, nn
492      inod = nodLOCAL(inod_e)
493      do jnod_e = 1, nn
494        jnod = nodLOCAL(jnod_e)
495        !***** Add components
496        call stf_get_block(stiffness, ndof, inod_e, jnod_e, a)
497        call hecmw_cmat_add(hecMAT%cmat, inod, jnod, a)
498      enddo
499    enddo
500    call hecmw_cmat_pack(hecMAT%cmat)
501
502  end subroutine hecmw_mat_ass_contact
503
504end module hecmw_matrix_ass
505