Add basic rigid plane contact support
This commit is contained in:
@@ -0,0 +1,135 @@
|
||||
module fem_contact
|
||||
use fem_precision
|
||||
use fem_error
|
||||
use fem_database
|
||||
implicit none
|
||||
|
||||
private
|
||||
|
||||
public :: contact_apply_plane_forces
|
||||
|
||||
contains
|
||||
|
||||
subroutine contact_apply_plane_forces(ierr)
|
||||
integer, intent(out) :: ierr
|
||||
integer :: plane
|
||||
|
||||
if (.not. allocated(fcontact)) then
|
||||
call error_set(FEM_ERR_INTERNAL, 'contact force vector is not allocated', ierr)
|
||||
return
|
||||
end if
|
||||
|
||||
fcontact = ZERO
|
||||
do plane = 1, ncontact_plane
|
||||
call apply_contact_plane(plane, ierr)
|
||||
if (ierr .ne. FEM_SUCCESS) return
|
||||
end do
|
||||
|
||||
ierr = FEM_SUCCESS
|
||||
end subroutine contact_apply_plane_forces
|
||||
|
||||
subroutine apply_contact_plane(plane, ierr)
|
||||
integer, intent(in) :: plane
|
||||
integer, intent(out) :: ierr
|
||||
integer :: node
|
||||
integer :: set_id
|
||||
integer :: pos
|
||||
integer :: external_id
|
||||
integer :: ios
|
||||
character(len=NAME_LEN) :: target
|
||||
|
||||
target = adjustl(contact_target(plane))
|
||||
if (len_trim(target) .eq. 0 .or. trim(target) .eq. 'ALL' .or. &
|
||||
trim(target) .eq. 'all') then
|
||||
do node = 1, nnode
|
||||
call apply_contact_plane_node(plane, node, ierr)
|
||||
if (ierr .ne. FEM_SUCCESS) return
|
||||
end do
|
||||
ierr = FEM_SUCCESS
|
||||
return
|
||||
end if
|
||||
|
||||
read(target,*,iostat=ios) external_id
|
||||
if (ios .eq. 0) then
|
||||
node = database_find_node(external_id)
|
||||
if (node .eq. 0) then
|
||||
call error_set(FEM_ERR_INPUT, 'contact plane references unknown node', ierr)
|
||||
return
|
||||
end if
|
||||
call apply_contact_plane_node(plane, node, ierr)
|
||||
return
|
||||
end if
|
||||
|
||||
set_id = database_find_nset(target)
|
||||
if (set_id .eq. 0) then
|
||||
call error_set(FEM_ERR_INPUT, 'contact plane references unknown node set', ierr)
|
||||
return
|
||||
end if
|
||||
|
||||
do pos = nset_start(set_id), nset_start(set_id) + nset_count(set_id) - 1
|
||||
node = database_find_node(nset_items(pos))
|
||||
if (node .eq. 0) then
|
||||
call error_set(FEM_ERR_INPUT, 'contact node set contains unknown node', ierr)
|
||||
return
|
||||
end if
|
||||
call apply_contact_plane_node(plane, node, ierr)
|
||||
if (ierr .ne. FEM_SUCCESS) return
|
||||
end do
|
||||
|
||||
ierr = FEM_SUCCESS
|
||||
end subroutine apply_contact_plane
|
||||
|
||||
subroutine apply_contact_plane_node(plane, node, ierr)
|
||||
integer, intent(in) :: plane
|
||||
integer, intent(in) :: node
|
||||
integer, intent(out) :: ierr
|
||||
real(rk) :: normal(NDIM)
|
||||
real(rk) :: position(NDIM)
|
||||
real(rk) :: velocity(NDIM)
|
||||
real(rk) :: norm_n
|
||||
real(rk) :: offset
|
||||
real(rk) :: gap
|
||||
real(rk) :: vn
|
||||
real(rk) :: force_mag
|
||||
integer :: idir
|
||||
integer :: dof
|
||||
|
||||
if (node .lt. 1 .or. node .gt. nnode) then
|
||||
call error_set(FEM_ERR_INPUT, 'contact node is out of range', ierr)
|
||||
return
|
||||
end if
|
||||
|
||||
normal = contact_normal(:,plane)
|
||||
norm_n = sqrt(sum(normal * normal))
|
||||
if (norm_n .le. tiny(ONE)) then
|
||||
call error_set(FEM_ERR_INPUT, 'contact plane normal must be non-zero', ierr)
|
||||
return
|
||||
end if
|
||||
normal = normal / norm_n
|
||||
offset = contact_offset(plane) / norm_n
|
||||
|
||||
position = coord0(:,node)
|
||||
velocity = ZERO
|
||||
do idir = 1, NDIM
|
||||
dof = dof_id(idir,node)
|
||||
if (dof .gt. 0) then
|
||||
position(idir) = position(idir) + u(dof)
|
||||
velocity(idir) = v(dof)
|
||||
end if
|
||||
end do
|
||||
|
||||
gap = dot_product(normal, position) - offset
|
||||
if (gap .lt. ZERO) then
|
||||
vn = dot_product(normal, velocity)
|
||||
force_mag = contact_penalty(plane) * (-gap)
|
||||
if (vn .lt. ZERO) force_mag = force_mag + contact_damping(plane) * (-vn)
|
||||
do idir = 1, NDIM
|
||||
dof = dof_id(idir,node)
|
||||
if (dof .gt. 0) fcontact(dof) = fcontact(dof) + force_mag * normal(idir)
|
||||
end do
|
||||
end if
|
||||
|
||||
ierr = FEM_SUCCESS
|
||||
end subroutine apply_contact_plane_node
|
||||
|
||||
end module fem_contact
|
||||
Reference in New Issue
Block a user