Add penalty MPC constraint support

This commit is contained in:
shenw
2026-06-22 23:25:35 +08:00
parent 81ba21af55
commit 473d97891c
6 changed files with 484 additions and 0 deletions
+60
View File
@@ -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