Add penalty MPC constraint support
This commit is contained in:
@@ -55,6 +55,23 @@ contains
|
||||
ierr = FEM_SUCCESS
|
||||
end subroutine boundary_apply_robin_forces
|
||||
|
||||
subroutine boundary_apply_mpc_forces(ierr)
|
||||
integer, intent(out) :: ierr
|
||||
integer :: eqn
|
||||
|
||||
if (.not. allocated(fext)) then
|
||||
call error_set(FEM_ERR_INTERNAL, 'external force vector is not allocated', ierr)
|
||||
return
|
||||
end if
|
||||
|
||||
do eqn = 1, nmpc
|
||||
call apply_mpc_equation(eqn, ierr)
|
||||
if (ierr .ne. FEM_SUCCESS) return
|
||||
end do
|
||||
|
||||
ierr = FEM_SUCCESS
|
||||
end subroutine boundary_apply_mpc_forces
|
||||
|
||||
subroutine boundary_apply_dirichlet(time, ierr)
|
||||
real(rk), intent(in) :: time
|
||||
integer, intent(out) :: ierr
|
||||
@@ -138,6 +155,49 @@ contains
|
||||
ierr = FEM_SUCCESS
|
||||
end subroutine apply_robin_item
|
||||
|
||||
subroutine apply_mpc_equation(eqn, ierr)
|
||||
integer, intent(in) :: eqn
|
||||
integer, intent(out) :: ierr
|
||||
real(rk) :: gap
|
||||
real(rk) :: force_scale
|
||||
integer :: item
|
||||
integer :: start_pos
|
||||
integer :: stop_pos
|
||||
integer :: node
|
||||
integer :: dof
|
||||
|
||||
start_pos = mpc_start(eqn)
|
||||
stop_pos = start_pos + mpc_count(eqn) - 1
|
||||
if (start_pos .lt. 1 .or. stop_pos .gt. nmpc_term) then
|
||||
call error_set(FEM_ERR_INPUT, 'MPC equation term range is invalid', ierr)
|
||||
return
|
||||
end if
|
||||
|
||||
gap = -mpc_rhs(eqn)
|
||||
do item = start_pos, stop_pos
|
||||
node = database_find_node(mpc_node(item))
|
||||
if (node .eq. 0) then
|
||||
call error_set(FEM_ERR_INPUT, 'MPC force references unknown node', ierr)
|
||||
return
|
||||
end if
|
||||
if (mpc_dof(item) .lt. 1 .or. mpc_dof(item) .gt. NDOF_NODE) then
|
||||
call error_set(FEM_ERR_INPUT, 'MPC force dof is outside 1..6', ierr)
|
||||
return
|
||||
end if
|
||||
dof = dof_id(mpc_dof(item),node)
|
||||
if (dof .gt. 0) gap = gap + mpc_coeff(item) * u(dof)
|
||||
end do
|
||||
|
||||
force_scale = -mpc_penalty(eqn) * gap
|
||||
do item = start_pos, stop_pos
|
||||
node = database_find_node(mpc_node(item))
|
||||
dof = dof_id(mpc_dof(item),node)
|
||||
if (dof .gt. 0) fext(dof) = fext(dof) + force_scale * mpc_coeff(item)
|
||||
end do
|
||||
|
||||
ierr = FEM_SUCCESS
|
||||
end subroutine apply_mpc_equation
|
||||
|
||||
subroutine apply_dload_item(item, ierr)
|
||||
integer, intent(in) :: item
|
||||
integer, intent(out) :: ierr
|
||||
|
||||
Reference in New Issue
Block a user