fstr_contact_smoothing.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_contact_smoothing
  use hecmw, only: kint, kreal
  use hecmw_util, only: hecmw_name_len, hecmwst_local_mesh
  use elementinfo
  use m_utilities, only: calinverse
  use msurfelement, only: tsurfelement
  implicit none
 
  ! Tikhonov regularization parameter
  real(kind=kreal), parameter, private :: nagata_epsilon = 1.0d-4
 
  integer(kind=kint), parameter, private :: DEBUG = 0
 
  private
  public :: compute_cab
  public :: compute_interpolation_matrix_p
  public :: update_surface_normal
  public :: create_intermediate_points
  public :: reorder_tri3n_to_tri6n
 
contains
 
  subroutine reorder_tri3n_to_tri6n(vertices_in, midpoints_in, nodes_out)
    real(kind=kreal), intent(in)  :: vertices_in(3,3)   ! fe_tri3n vertices
    real(kind=kreal), intent(in)  :: midpoints_in(3,3)  ! fe_tri3n midpoints
    real(kind=kreal), intent(out) :: nodes_out(3,6)     ! fe_tri6n nodes
    
    ! Reorder vertices: xi,et,st -> st,xi,et
    nodes_out(1:3, 1) = vertices_in(1:3, 3)  ! st
    nodes_out(1:3, 2) = vertices_in(1:3, 1)  ! xi
    nodes_out(1:3, 3) = vertices_in(1:3, 2)  ! et
    
    ! Reorder midpoints: xi-et,et-st,st-xi -> xi-st,xi-et,et-st
    nodes_out(1:3, 4) = midpoints_in(1:3, 3)  ! xi-st (was st-xi, same point)
    nodes_out(1:3, 5) = midpoints_in(1:3, 1)  ! xi-et
    nodes_out(1:3, 6) = midpoints_in(1:3, 2)  ! et-st
  end subroutine reorder_tri3n_to_tri6n
 
  subroutine reorder_normals_tri3n_to_tri6n(normals_in, normals_out)
    real(kind=kreal), intent(in)  :: normals_in(3,3)   ! fe_tri3n order
    real(kind=kreal), intent(out) :: normals_out(3,3)  ! fe_tri6n order
    
    ! xi,et,st -> st,xi,et
    normals_out(1:3, 1) = normals_in(1:3, 3)  ! st
    normals_out(1:3, 2) = normals_in(1:3, 1)  ! xi
    normals_out(1:3, 3) = normals_in(1:3, 2)  ! et
  end subroutine reorder_normals_tri3n_to_tri6n
 
  subroutine update_surface_normal( surf, currpos, hecMESH )
    use msurfelement, only: calc_all_surf_vertex_normals
    use m_hecmw_comm_f, only: hecmw_update_r, hecmw_assemble_r
    type(tsurfelement), intent(inout) :: surf(:)
    real(kind=kreal), intent(in) :: currpos(:)      
    type(hecmwst_local_mesh), intent(in), optional :: hecmesh
 
    integer(kind=kint) :: i, j, nn, gnode, n_node
    real(kind=kreal) :: normal(3), norm_len
    real(kind=kreal), allocatable :: vnormal(:)
    
    ! Calculate geometric normals at vertices (skip if no local surf elements)
    if (size(surf) > 0) then
      call calc_all_surf_vertex_normals(surf, currpos)
    endif
    
    ! Determine array size
    if (present(hecmesh)) then
      n_node = hecmesh%n_node
    else
      if (size(surf) == 0) return
      n_node = maxval([(maxval(surf(i)%nodes), i=1,size(surf))])
    endif
 
    ! Allocate flat array for MPI communication: vnormal(3*n_node)
    allocate(vnormal(3*n_node))
    vnormal = 0.0d0
    
    ! Accumulate vertex normals from all local surface elements
    do i = 1, size(surf)
      nn = size(surf(i)%nodes)
      do j = 1, nn
        gnode = surf(i)%nodes(j)
        vnormal(3*(gnode-1)+1:3*(gnode-1)+3) = &
          vnormal(3*(gnode-1)+1:3*(gnode-1)+3) &
          + surf(i)%vertex_normals(:, j)
      enddo
    enddo
    
    ! MPI communication: allreduce (sum) across processes
    ! All processes must participate even if they have no local surf elements
    if (present(hecmesh)) then
      call hecmw_assemble_r(hecmesh, vnormal, n_node, 3)
      call hecmw_update_r(hecmesh, vnormal, n_node, 3)
    endif
    
    ! Normalize and write back to surf%vertex_normals
    do i = 1, size(surf)
      nn = size(surf(i)%nodes)
      do j = 1, nn
        gnode = surf(i)%nodes(j)
        normal(:) = vnormal(3*(gnode-1)+1:3*(gnode-1)+3)
        norm_len = dsqrt(dot_product(normal, normal))
        if (norm_len > 1.0d-20) then
          surf(i)%vertex_normals(:, j) = normal / norm_len
        endif
      enddo
    enddo
    
    deallocate(vnormal)
 
  end subroutine update_surface_normal
 
  subroutine compute_cab_old(na_vec, nb_vec, Cab_a, Cab_b)
    implicit none
    real(kind=kreal), intent(in)  :: na_vec(3)   ! unit normal at vertex a
    real(kind=kreal), intent(in)  :: nb_vec(3)   ! unit normal at vertex b
    real(kind=kreal), intent(out) :: cab_a(3,3)  ! correction matrix for vertex a
    real(kind=kreal), intent(out) :: cab_b(3,3)  ! correction matrix for vertex b
 
    real(kind=kreal) :: aab(2,3), ata(3,3), ata_orig(3,3), ata_inv(3,3)
    real(kind=kreal) :: p(3,3)
    real(kind=kreal) :: epsilon
    integer(kind=kint) :: i
 
    ! Build Aab = [na^T; nb^T]
    aab(1,:) = na_vec(:)
    aab(2,:) = nb_vec(:)
 
    ! Compute AtA = Aab^T * Aab
    ata = matmul(transpose(aab), aab)
    ata_orig = ata
 
    ! Regularize: AtA_reg = AtA + eps*I
    epsilon = (ata(1,1)+ata(2,2)+ata(3,3)) * nagata_epsilon
    do i = 1, 3
      ata(i,i) = ata(i,i) + epsilon
    enddo
 
    ! Invert AtA_reg using calInverse
    ata_inv = ata
    call calinverse(3, ata_inv)
 
    ! Compute P = AtA_inv * AtA_orig
    p = matmul(ata_inv, ata_orig)
 
    ! Cab_a = 0.5*I + 0.25*P
    cab_a = 0.25d0 * p
    do i = 1, 3
      cab_a(i,i) = cab_a(i,i) + 0.5d0
    enddo
 
    ! Cab_b = 0.5*I - 0.25*P
    cab_b = -0.25d0 * p
    do i = 1, 3
      cab_b(i,i) = cab_b(i,i) + 0.5d0
    enddo
 
  end subroutine compute_cab_old
 
  subroutine compute_cab(na_vec, nb_vec, Cab_a, Cab_b)
    implicit none
    real(kind=kreal), intent(in)  :: na_vec(3)   ! unit normal at vertex a (n1)
    real(kind=kreal), intent(in)  :: nb_vec(3)   ! unit normal at vertex b (n2)
    real(kind=kreal), intent(out) :: cab_a(3,3)
    real(kind=kreal), intent(out) :: cab_b(3,3)
 
    real(kind=kreal) :: i3(3,3)
    real(kind=kreal) :: m(3), mnorm
    real(kind=kreal) :: a1, a2
    real(kind=kreal) :: beta, den, k
    real(kind=kreal) :: r(3)      ! r = a2*nb - a1*na
    real(kind=kreal) :: q(3,3)    ! outer(m, r)
    integer(kind=kint) :: i, j
 
    ! Identity
    i3 = 0.0d0
    do i=1,3
      i3(i,i) = 1.0d0
    enddo
 
    ! m = normalize(na + nb)
    m(:) = na_vec(:) + nb_vec(:)
    mnorm = sqrt( m(1)*m(1) + m(2)*m(2) + m(3)*m(3) )
 
    if (mnorm < 1.0d-12) then
      m(:) = na_vec(:)
      mnorm = 1.0d0
    else
      m(:) = m(:) / mnorm
    endif
 
    a1 = m(1)*na_vec(1) + m(2)*na_vec(2) + m(3)*na_vec(3)
    a2 = m(1)*nb_vec(1) + m(2)*nb_vec(2) + m(3)*nb_vec(3)
 
    ! Regularization for alpha (beta >= 0)
    ! Option 1: fixed dimensionless parameter
    beta = nagata_epsilon   ! << define e.g. 1e-3 .. 1e-1 (see notes below)
 
    den = 16.0d0*(a1*a1 + a2*a2) + beta
 
    ! If den too small, do no curvature (midpoint)
    if (den < 1.0d-20) then
      cab_a = 0.5d0 * i3
      cab_b = 0.5d0 * i3
      return
    endif
 
    k = 4.0d0 / den
 
    ! r = a2*nb - a1*na  (3-vector)
    r(:) = a2*nb_vec(:) - a1*na_vec(:)
 
    ! Q = outer(m, r) = m * r^T  (3x3)
    do i=1,3
      do j=1,3
        q(i,j) = m(i) * r(j)
      enddo
    enddo
 
    ! Cab_a = 0.5*I - K*Q
    ! Cab_b = 0.5*I + K*Q
    cab_a = 0.5d0*i3 - k*q
    cab_b = 0.5d0*i3 + k*q
 
  end subroutine compute_cab
 
  subroutine compute_interpolation_matrix_p(etype, nnode, lpos, vertex_normals, P_matrix)
    implicit none
    integer(kind=kint), intent(in)  :: etype          ! element type
    integer(kind=kint), intent(in)  :: nnode          ! number of nodes
    real(kind=kreal), intent(in)    :: lpos(2)        ! local coordinates (xi, eta)
    real(kind=kreal), intent(in)    :: vertex_normals(3,nnode)  ! unit normals at vertices
    real(kind=kreal), intent(out)   :: p_matrix(3,nnode*3)      ! interpolation matrix
 
    real(kind=kreal) :: n(8)
    real(kind=kreal) :: cab_12_1(3,3), cab_12_2(3,3)
    real(kind=kreal) :: cab_23_2(3,3), cab_23_3(3,3)
    real(kind=kreal) :: cab_31_3(3,3), cab_31_1(3,3)
    real(kind=kreal) :: cab_34_3(3,3), cab_34_4(3,3)
    real(kind=kreal) :: cab_41_4(3,3), cab_41_1(3,3)
    real(kind=kreal) :: p1(3,3), p2(3,3), p3(3,3), p4(3,3)
    real(kind=kreal) :: i3(3,3)
    real(kind=kreal) :: normals_tri6n(3,3)
    integer(kind=kint) :: i, j
 
    p_matrix = 0.0d0
 
    ! Identity matrix
    i3 = 0.0d0
    i3(1,1) = 1.0d0
    i3(2,2) = 1.0d0
    i3(3,3) = 1.0d0
 
    select case(etype)
    case(fe_tri3n)
      call getshapefunc(fe_tri6n, lpos, n)
      call reorder_normals_tri3n_to_tri6n(vertex_normals, normals_tri6n)
      
      ! Compute Cab for fe_tri6n edges: (st-xi), (xi-et), (et-st)
      call compute_cab(normals_tri6n(:,1), normals_tri6n(:,2), cab_31_3, cab_31_1)
      call compute_cab(normals_tri6n(:,2), normals_tri6n(:,3), cab_12_1, cab_12_2)
      call compute_cab(normals_tri6n(:,3), normals_tri6n(:,1), cab_23_2, cab_23_3)
 
      ! P matrices for fe_tri6n: st, xi, et
      p1 = n(1) * i3 + n(4) * cab_31_3 + n(6) * cab_23_3
      p2 = n(2) * i3 + n(4) * cab_31_1 + n(5) * cab_12_1
      p3 = n(3) * i3 + n(5) * cab_12_2 + n(6) * cab_23_2
 
      ! Assemble P in fe_tri3n order: xi, et, st
      p_matrix(1:3, 1:3) = p2
      p_matrix(1:3, 4:6) = p3
      p_matrix(1:3, 7:9) = p1
 
    case(fe_quad4n)
      call getshapefunc(fe_quad8n, lpos, n)
      
      ! Compute Cab for 4 edges: (1,2), (2,3), (3,4), (4,1)
      call compute_cab(vertex_normals(:,1), vertex_normals(:,2), cab_12_1, cab_12_2)
      call compute_cab(vertex_normals(:,2), vertex_normals(:,3), cab_23_2, cab_23_3)
      call compute_cab(vertex_normals(:,3), vertex_normals(:,4), cab_34_3, cab_34_4)
      call compute_cab(vertex_normals(:,4), vertex_normals(:,1), cab_41_4, cab_41_1)
 
      ! P matrices for fe_quad8n: node 1, 2, 3, 4
      p1 = n(1) * i3 + n(5) * cab_12_1 + n(8) * cab_41_1
      p2 = n(2) * i3 + n(5) * cab_12_2 + n(6) * cab_23_2
      p3 = n(3) * i3 + n(6) * cab_23_3 + n(7) * cab_34_3
      p4 = n(4) * i3 + n(7) * cab_34_4 + n(8) * cab_41_4
 
      ! Assemble P in fe_quad4n order: 1, 2, 3, 4
      p_matrix(1:3, 1:3) = p1
      p_matrix(1:3, 4:6) = p2
      p_matrix(1:3, 7:9) = p3
      p_matrix(1:3, 10:12) = p4
 
    case default
      write(*,*) "Error: compute_interpolation_matrix_P - Unsupported element type for Nagata patch.",etype
      stop 
    end select
 
  end subroutine compute_interpolation_matrix_p
 
  subroutine create_intermediate_points( surf, currpos, contact_name )
    type(tsurfelement), intent(inout) :: surf(:)
    real(kind=kreal), intent(in) :: currpos(:)      
    character(len=HECMW_NAME_LEN), intent(in) :: contact_name
 
    integer(kind=kint) :: i
    
    if (size(surf) == 0) return
 
    !$omp parallel do private(i)
    do i = 1, size(surf)
      call create_intermediate_points_single(surf(i), currpos)
    enddo
    !$omp end parallel do
 
    if( debug > 0 ) then
      call print_surface_elements_to_vtk(surf, currpos, contact_name)
    end if  
 
  end subroutine create_intermediate_points
 
  subroutine create_intermediate_points_single( surf, currpos )
    type(tsurfelement), intent(inout) :: surf
    real(kind=kreal), intent(in) :: currpos(:)      
    
    integer(kind=kint) :: i, etype
    real(kind=kreal) :: elem(3,4)
    real(kind=kreal), pointer :: vertex_normals(:,:)
    real(kind=kreal) :: cab_12_1(3,3), cab_12_2(3,3)
    real(kind=kreal) :: cab_23_2(3,3), cab_23_3(3,3)
    real(kind=kreal) :: cab_31_3(3,3), cab_31_1(3,3)
    real(kind=kreal) :: cab_34_3(3,3), cab_34_4(3,3)
    real(kind=kreal) :: cab_41_4(3,3), cab_41_1(3,3)
 
    etype = surf%etype
    vertex_normals => surf%vertex_normals
 
    select case(etype)
    case(fe_tri3n)
 
      if (.not. associated(surf%intermediate_points)) then
        allocate(surf%intermediate_points(3,3))
      end if
 
      do i=1,3
        elem(1:3,i) = currpos(3*(surf%nodes(i)-1)+1 : 3*surf%nodes(i))
      enddo
 
      ! Compute Cab for 3 edges: (1,2), (2,3), (3,1)
      call compute_cab(vertex_normals(:,1), vertex_normals(:,2), cab_12_1, cab_12_2)
      call compute_cab(vertex_normals(:,2), vertex_normals(:,3), cab_23_2, cab_23_3)
      call compute_cab(vertex_normals(:,3), vertex_normals(:,1), cab_31_3, cab_31_1)
 
      ! intermediate point
      surf%intermediate_points(:,1) = matmul(cab_12_1(1:3,1:3), elem(1:3,1)) + matmul(cab_12_2(1:3,1:3), elem(1:3,2))
      surf%intermediate_points(:,2) = matmul(cab_23_2(1:3,1:3), elem(1:3,2)) + matmul(cab_23_3(1:3,1:3), elem(1:3,3))
      surf%intermediate_points(:,3) = matmul(cab_31_3(1:3,1:3), elem(1:3,3)) + matmul(cab_31_1(1:3,1:3), elem(1:3,1))
 
    case(fe_quad4n)
 
      if (.not. associated(surf%intermediate_points)) then
        allocate(surf%intermediate_points(3,4))
      end if
 
      do i=1,4
        elem(1:3,i) = currpos(3*(surf%nodes(i)-1)+1 : 3*surf%nodes(i))
      enddo
 
      ! Compute Cab for 4 edges: (1,2), (2,3), (3,4), (4,1)
      call compute_cab(vertex_normals(:,1), vertex_normals(:,2), cab_12_1, cab_12_2)
      call compute_cab(vertex_normals(:,2), vertex_normals(:,3), cab_23_2, cab_23_3)
      call compute_cab(vertex_normals(:,3), vertex_normals(:,4), cab_34_3, cab_34_4)
      call compute_cab(vertex_normals(:,4), vertex_normals(:,1), cab_41_4, cab_41_1)
 
      ! intermediate points
      surf%intermediate_points(:,1) = matmul(cab_12_1(1:3,1:3), elem(1:3,1)) + matmul(cab_12_2(1:3,1:3), elem(1:3,2))
      surf%intermediate_points(:,2) = matmul(cab_23_2(1:3,1:3), elem(1:3,2)) + matmul(cab_23_3(1:3,1:3), elem(1:3,3))
      surf%intermediate_points(:,3) = matmul(cab_34_3(1:3,1:3), elem(1:3,3)) + matmul(cab_34_4(1:3,1:3), elem(1:3,4))
      surf%intermediate_points(:,4) = matmul(cab_41_4(1:3,1:3), elem(1:3,4)) + matmul(cab_41_1(1:3,1:3), elem(1:3,1))
 
    case default
      write(*,*) "Error: create_intermediate_points - Unsupported element type for Nagata patch.",etype
      stop 
    end select
 
  end subroutine create_intermediate_points_single
  
  subroutine print_surface_elements_to_vtk( surf, currpos, contact_name )
    type(tsurfelement), intent(in) :: surf(:)
    real(kind=kreal), intent(in) :: currpos(:)   
    character(len=HECMW_NAME_LEN), intent(in) :: contact_name
 
    integer(kind=kint), parameter :: vtk_unit = 99
    integer(kind=kint) :: i, j, nn, n_tri, n_quad, n_elem, n_points, n_cells_size, node_id, inode
    real(kind=kreal) :: coord(3), normal(3)
    character(len=256) :: filename
 
    ! Count valid elements
    n_tri = 0
    n_quad = 0
    do i = 1, size(surf)
      if (surf(i)%etype == fe_tri3n) then
        n_tri = n_tri + 1
      else if (surf(i)%etype == fe_quad4n) then
        n_quad = n_quad + 1
      endif
    enddo
 
    n_elem = n_tri + n_quad
    if (n_elem == 0) return
 
    ! Calculate total points and cells size
    n_points = n_tri * 6 + n_quad * 8
    n_cells_size = n_tri * 7 + n_quad * 9  ! tri: 1+6, quad: 1+8
 
    ! Open VTK file
    filename = trim(contact_name) // '_nagata_debug.vtk'
    open(unit=vtk_unit, file=filename, status='replace', action='write', form='formatted')
 
    ! Write header
    write(vtk_unit, '(A)') '# vtk DataFile Version 3.0'
    write(vtk_unit, '(A,A)') 'Nagata patch debug: ', trim(contact_name)
    write(vtk_unit, '(A)') 'ASCII'
    write(vtk_unit, '(A)') 'DATASET UNSTRUCTURED_GRID'
    write(vtk_unit, *)
 
    ! Write POINTS
    write(vtk_unit, '(A,I0,A)') 'POINTS ', n_points, ' float'
    do i = 1, size(surf)
      if (surf(i)%etype == fe_tri3n .or. surf(i)%etype == fe_quad4n) then
        nn = size(surf(i)%nodes)
        ! Output vertex coordinates
        do j = 1, nn
          inode = surf(i)%nodes(j)
          coord(1:3) = currpos(3*(inode-1)+1 : 3*inode)
          write(vtk_unit, '(3(E15.7,1X))') coord(1), coord(2), coord(3)
        enddo
        ! Output intermediate point coordinates
        do j = 1, nn
          coord(1:3) = surf(i)%intermediate_points(1:3, j)
          write(vtk_unit, '(3(E15.7,1X))') coord(1), coord(2), coord(3)
        enddo
      endif
    enddo
    write(vtk_unit, *)
 
    ! Write CELLS
    write(vtk_unit, '(A,I0,1X,I0)') 'CELLS ', n_elem, n_cells_size
    node_id = 0
    do i = 1, size(surf)
      if (surf(i)%etype == fe_tri3n) then
        write(vtk_unit, '(I0,1X,I0,1X,I0,1X,I0,1X,I0,1X,I0,1X,I0)') &
          6, node_id, node_id+1, node_id+2, node_id+3, node_id+4, node_id+5
        node_id = node_id + 6
      else if (surf(i)%etype == fe_quad4n) then
        write(vtk_unit, '(I0,1X,I0,1X,I0,1X,I0,1X,I0,1X,I0,1X,I0,1X,I0,1X,I0)') &
          8, node_id, node_id+1, node_id+2, node_id+3, node_id+4, node_id+5, node_id+6, node_id+7
        node_id = node_id + 8
      endif
    enddo
    write(vtk_unit, *)
 
    ! Write CELL_TYPES
    write(vtk_unit, '(A,I0)') 'CELL_TYPES ', n_elem
    do i = 1, size(surf)
      if (surf(i)%etype == fe_tri3n) then
        write(vtk_unit, '(I0)') 22  ! VTK_QUADRATIC_TRIANGLE
      else if (surf(i)%etype == fe_quad4n) then
        write(vtk_unit, '(I0)') 23  ! VTK_QUADRATIC_QUAD
      endif
    enddo
    write(vtk_unit, *)
 
    ! Write POINT_DATA - VECTORS (vertex normals)
    write(vtk_unit, '(A,I0)') 'POINT_DATA ', n_points
    write(vtk_unit, '(A)') 'VECTORS vertex_normal float'
    do i = 1, size(surf)
      if (surf(i)%etype == fe_tri3n .or. surf(i)%etype == fe_quad4n) then
        nn = size(surf(i)%nodes)
        ! Output vertex normals
        do j = 1, nn
          normal(1:3) = surf(i)%vertex_normals(1:3, j)
          write(vtk_unit, '(3(E15.7,1X))') normal(1), normal(2), normal(3)
        enddo
        ! Output zero normals for intermediate points
        do j = 1, nn
          write(vtk_unit, '(A)') '0.0 0.0 0.0'
        enddo
      endif
    enddo
 
    close(vtk_unit)
 
    write(*,'(A,A)') 'VTK debug file written: ', trim(filename)
 
  end subroutine print_surface_elements_to_vtk
 
 
end module m_fstr_contact_smoothing