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
+89
View File
@@ -62,6 +62,9 @@ contains
call apply_implicit_robin(kmat, rhs, time, ierr)
if (ierr .ne. FEM_SUCCESS) go to 900
call apply_implicit_mpc(kmat, rhs, ierr)
if (ierr .ne. FEM_SUCCESS) go to 900
call apply_implicit_dirichlet(kmat, rhs, time, ierr)
if (ierr .ne. FEM_SUCCESS) go to 900
@@ -219,6 +222,92 @@ contains
ierr = FEM_SUCCESS
end subroutine apply_robin_node
subroutine apply_implicit_mpc(kmat, rhs, ierr)
type(csr_matrix), intent(inout) :: kmat
real(rk), intent(inout) :: rhs(:)
integer, intent(out) :: ierr
integer :: eqn
do eqn = 1, nmpc
call apply_mpc_equation_implicit(kmat, rhs, eqn, ierr)
if (ierr .ne. FEM_SUCCESS) return
end do
ierr = FEM_SUCCESS
end subroutine apply_implicit_mpc
subroutine apply_mpc_equation_implicit(kmat, rhs, eqn, ierr)
type(csr_matrix), intent(inout) :: kmat
real(rk), intent(inout) :: rhs(:)
integer, intent(in) :: eqn
integer, intent(out) :: ierr
integer :: row_item
integer :: col_item
integer :: start_pos
integer :: stop_pos
integer :: row
integer :: col
real(rk) :: penalty
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, 'implicit MPC equation term range is invalid', ierr)
return
end if
penalty = mpc_penalty(eqn)
if (penalty .le. ZERO) then
call error_set(FEM_ERR_INPUT, 'implicit MPC penalty must be positive', ierr)
return
end if
do row_item = start_pos, stop_pos
call mpc_term_global_dof(row_item, row, ierr)
if (ierr .ne. FEM_SUCCESS) return
if (row .le. 0) cycle
rhs(row) = rhs(row) + penalty * mpc_rhs(eqn) * mpc_coeff(row_item)
do col_item = start_pos, stop_pos
call mpc_term_global_dof(col_item, col, ierr)
if (ierr .ne. FEM_SUCCESS) return
if (col .gt. 0) then
call csr_add_value(kmat, row, col, &
penalty * mpc_coeff(row_item) * mpc_coeff(col_item), ierr)
if (ierr .ne. FEM_SUCCESS) return
end if
end do
end do
ierr = FEM_SUCCESS
end subroutine apply_mpc_equation_implicit
subroutine mpc_term_global_dof(item, dof, ierr)
integer, intent(in) :: item
integer, intent(out) :: dof
integer, intent(out) :: ierr
integer :: node
dof = 0
if (item .lt. 1 .or. item .gt. nmpc_term) then
call error_set(FEM_ERR_INPUT, 'implicit MPC term index is out of range', ierr)
return
end if
if (mpc_dof(item) .lt. 1 .or. mpc_dof(item) .gt. NDOF_NODE) then
call error_set(FEM_ERR_INPUT, 'implicit MPC term dof is outside 1..6', ierr)
return
end if
node = database_find_node(mpc_node(item))
if (node .eq. 0) then
call error_set(FEM_ERR_INPUT, 'implicit MPC term references unknown node', ierr)
return
end if
dof = dof_id(mpc_dof(item),node)
ierr = FEM_SUCCESS
end subroutine mpc_term_global_dof
subroutine apply_bc_target(kmat, rhs, target, dof_num, value, ierr)
type(csr_matrix), intent(inout) :: kmat
real(rk), intent(inout) :: rhs(:)