From 473d97891c9451af46e27e8b8dca8463ef33de30 Mon Sep 17 00:00:00 2001 From: shenw Date: Mon, 22 Jun 2026 23:25:35 +0800 Subject: [PATCH] Add penalty MPC constraint support --- src/base/fem_api.f90 | 3 + src/boundary/fem_boundary.f90 | 60 ++++++++++++ src/mesh/fem_database.f90 | 135 ++++++++++++++++++++++++++ src/solver/fem_assembly_csr.f90 | 99 +++++++++++++++++++ src/solver/fem_solver_implicit.f90 | 89 +++++++++++++++++ tests/module/test_solver_implicit.f90 | 98 +++++++++++++++++++ 6 files changed, 484 insertions(+) diff --git a/src/base/fem_api.f90 b/src/base/fem_api.f90 index 9825f30..8dfc5e3 100644 --- a/src/base/fem_api.f90 +++ b/src/base/fem_api.f90 @@ -761,6 +761,9 @@ contains call boundary_apply_robin_forces(current_time, ierr) if (ierr .ne. FEM_SUCCESS) return + call boundary_apply_mpc_forces(ierr) + if (ierr .ne. FEM_SUCCESS) return + do dof = 1, ndof rhs = fext(dof) + fsph(dof) + fcontact(dof) - fint(dof) a(dof) = rhs / mass_diag(dof) diff --git a/src/boundary/fem_boundary.f90 b/src/boundary/fem_boundary.f90 index f984dee..c7ee066 100644 --- a/src/boundary/fem_boundary.f90 +++ b/src/boundary/fem_boundary.f90 @@ -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 diff --git a/src/mesh/fem_database.f90 b/src/mesh/fem_database.f90 index 34c8498..d0df3d1 100644 --- a/src/mesh/fem_database.f90 +++ b/src/mesh/fem_database.f90 @@ -19,6 +19,8 @@ module fem_database integer :: ncload = 0 integer :: ndload = 0 integer :: nrobin = 0 + integer :: nmpc = 0 + integer :: nmpc_term = 0 real(rk), allocatable :: coord0(:,:) real(rk), allocatable :: coord(:,:) @@ -123,6 +125,14 @@ module fem_database real(rk), allocatable :: robin_reference(:) integer, allocatable :: robin_amp(:) + integer, allocatable :: mpc_start(:) + integer, allocatable :: mpc_count(:) + real(rk), allocatable :: mpc_rhs(:) + real(rk), allocatable :: mpc_penalty(:) + integer, allocatable :: mpc_node(:) + integer, allocatable :: mpc_dof(:) + real(rk), allocatable :: mpc_coeff(:) + contains subroutine database_allocate_mesh(num_nodes, num_elems, ierr) @@ -474,6 +484,68 @@ contains ierr = FEM_SUCCESS end subroutine database_allocate_robin + subroutine database_allocate_mpc(num_mpc, num_terms, ierr) + integer, intent(in) :: num_mpc + integer, intent(in) :: num_terms + integer, intent(out) :: ierr + integer :: stat + + if (num_mpc .lt. 0 .or. num_terms .lt. 0) then + call error_set(FEM_ERR_INPUT, 'number of MPC equations and terms must be non-negative', ierr) + return + end if + if (num_mpc .eq. 0 .and. num_terms .ne. 0) then + call error_set(FEM_ERR_INPUT, 'MPC terms require at least one equation', ierr) + return + end if + + if (allocated(mpc_start)) deallocate(mpc_start) + if (allocated(mpc_count)) deallocate(mpc_count) + if (allocated(mpc_rhs)) deallocate(mpc_rhs) + if (allocated(mpc_penalty)) deallocate(mpc_penalty) + if (allocated(mpc_node)) deallocate(mpc_node) + if (allocated(mpc_dof)) deallocate(mpc_dof) + if (allocated(mpc_coeff)) deallocate(mpc_coeff) + + nmpc = num_mpc + nmpc_term = num_terms + + if (num_mpc .gt. 0) then + allocate(mpc_start(num_mpc), mpc_count(num_mpc), mpc_rhs(num_mpc), & + mpc_penalty(num_mpc), stat=stat) + if (stat .ne. 0) then + nmpc = 0 + nmpc_term = 0 + call error_set(FEM_ERR_INTERNAL, 'failed to allocate MPC headers', ierr) + return + end if + mpc_start = 0 + mpc_count = 0 + mpc_rhs = ZERO + mpc_penalty = ZERO + end if + + if (num_terms .gt. 0) then + allocate(mpc_node(num_terms), mpc_dof(num_terms), & + mpc_coeff(num_terms), stat=stat) + if (stat .ne. 0) then + if (allocated(mpc_start)) deallocate(mpc_start) + if (allocated(mpc_count)) deallocate(mpc_count) + if (allocated(mpc_rhs)) deallocate(mpc_rhs) + if (allocated(mpc_penalty)) deallocate(mpc_penalty) + nmpc = 0 + nmpc_term = 0 + call error_set(FEM_ERR_INTERNAL, 'failed to allocate MPC terms', ierr) + return + end if + mpc_node = 0 + mpc_dof = 0 + mpc_coeff = ZERO + end if + + ierr = FEM_SUCCESS + end subroutine database_allocate_mpc + subroutine database_allocate_basic(num_nodes, num_elems, num_dof, ierr) integer, intent(in) :: num_nodes integer, intent(in) :: num_elems @@ -642,6 +714,13 @@ contains if (allocated(robin_damping)) robin_damping = ZERO if (allocated(robin_reference)) robin_reference = ZERO if (allocated(robin_amp)) robin_amp = 0 + if (allocated(mpc_start)) mpc_start = 0 + if (allocated(mpc_count)) mpc_count = 0 + if (allocated(mpc_rhs)) mpc_rhs = ZERO + if (allocated(mpc_penalty)) mpc_penalty = ZERO + if (allocated(mpc_node)) mpc_node = 0 + if (allocated(mpc_dof)) mpc_dof = 0 + if (allocated(mpc_coeff)) mpc_coeff = ZERO ierr = FEM_SUCCESS end subroutine database_initialize @@ -867,6 +946,9 @@ contains if (ierr .ne. FEM_SUCCESS) return call validate_robin_data(ierr) + if (ierr .ne. FEM_SUCCESS) return + + call validate_mpc_data(ierr) end subroutine database_validate_model subroutine validate_materials(ierr) @@ -1157,6 +1239,50 @@ contains ierr = FEM_SUCCESS end subroutine validate_robin_data + subroutine validate_mpc_data(ierr) + integer, intent(out) :: ierr + integer :: eqn + integer :: item + integer :: start_pos + integer :: stop_pos + integer :: node + + do eqn = 1, nmpc + start_pos = mpc_start(eqn) + stop_pos = start_pos + mpc_count(eqn) - 1 + if (mpc_count(eqn) .le. 0) then + call error_set(FEM_ERR_INPUT, 'MPC equation has no terms', ierr) + return + end if + 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 + if (mpc_penalty(eqn) .le. ZERO) then + call error_set(FEM_ERR_INPUT, 'MPC penalty must be positive', ierr) + return + end if + + 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 term 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 term dof is outside 1..6', ierr) + return + end if + if (abs(mpc_coeff(item)) .le. tiny(ONE)) then + call error_set(FEM_ERR_INPUT, 'MPC term coefficient must be non-zero', ierr) + return + end if + end do + end do + + ierr = FEM_SUCCESS + end subroutine validate_mpc_data + logical function node_or_nset_exists(target) character(len=*), intent(in) :: target integer :: id @@ -1277,6 +1403,13 @@ contains if (allocated(robin_damping)) deallocate(robin_damping) if (allocated(robin_reference)) deallocate(robin_reference) if (allocated(robin_amp)) deallocate(robin_amp) + if (allocated(mpc_start)) deallocate(mpc_start) + if (allocated(mpc_count)) deallocate(mpc_count) + if (allocated(mpc_rhs)) deallocate(mpc_rhs) + if (allocated(mpc_penalty)) deallocate(mpc_penalty) + if (allocated(mpc_node)) deallocate(mpc_node) + if (allocated(mpc_dof)) deallocate(mpc_dof) + if (allocated(mpc_coeff)) deallocate(mpc_coeff) nnode = 0 nelem = 0 @@ -1293,6 +1426,8 @@ contains ncload = 0 ndload = 0 nrobin = 0 + nmpc = 0 + nmpc_term = 0 ierr = FEM_SUCCESS end subroutine database_finalize diff --git a/src/solver/fem_assembly_csr.f90 b/src/solver/fem_assembly_csr.f90 index cce38d3..60fd34f 100644 --- a/src/solver/fem_assembly_csr.f90 +++ b/src/solver/fem_assembly_csr.f90 @@ -125,6 +125,9 @@ contains end do end do + call count_mpc_pattern_entries(raw_count, ierr) + if (ierr .ne. FEM_SUCCESS) go to 900 + max_raw = maxval(raw_count) allocate(row_cols(max_raw, ndof), stat=stat) if (stat .ne. 0) then @@ -156,6 +159,9 @@ contains end do end do + call append_mpc_pattern_columns(row_cols, row_count, max_raw, ierr) + if (ierr .ne. FEM_SUCCESS) go to 900 + do row = 1, ndof call sort_row_columns(row_cols, row_count(row), row) end do @@ -294,6 +300,99 @@ contains ierr = FEM_SUCCESS end subroutine append_unique_column + subroutine count_mpc_pattern_entries(raw_count, ierr) + integer, intent(inout) :: raw_count(:) + integer, intent(out) :: ierr + integer :: eqn + integer :: item + integer :: start_pos + integer :: stop_pos + integer :: dof + + do eqn = 1, nmpc + 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_SOLVER, 'MPC pattern term range is invalid', ierr) + return + end if + + do item = start_pos, stop_pos + call mpc_term_dof(item, dof, ierr) + if (ierr .ne. FEM_SUCCESS) return + if (dof .gt. 0) raw_count(dof) = raw_count(dof) + mpc_count(eqn) + end do + end do + + ierr = FEM_SUCCESS + end subroutine count_mpc_pattern_entries + + subroutine append_mpc_pattern_columns(row_cols, row_count, max_raw, ierr) + integer, intent(inout) :: row_cols(:,:) + integer, intent(inout) :: row_count(:) + integer, intent(in) :: max_raw + integer, intent(out) :: ierr + integer :: eqn + integer :: row_item + integer :: col_item + integer :: start_pos + integer :: stop_pos + integer :: row + integer :: col + + do eqn = 1, nmpc + 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_SOLVER, 'MPC pattern term range is invalid', ierr) + return + end if + + do row_item = start_pos, stop_pos + call mpc_term_dof(row_item, row, ierr) + if (ierr .ne. FEM_SUCCESS) return + if (row .le. 0) cycle + + do col_item = start_pos, stop_pos + call mpc_term_dof(col_item, col, ierr) + if (ierr .ne. FEM_SUCCESS) return + if (col .gt. 0) then + call append_unique_column(row_cols, row_count, max_raw, row, col, ierr) + if (ierr .ne. FEM_SUCCESS) return + end if + end do + end do + end do + + ierr = FEM_SUCCESS + end subroutine append_mpc_pattern_columns + + subroutine mpc_term_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_SOLVER, '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_SOLVER, '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_SOLVER, 'MPC term references unknown node', ierr) + return + end if + + dof = dof_id(mpc_dof(item),node) + ierr = FEM_SUCCESS + end subroutine mpc_term_dof + subroutine sort_row_columns(row_cols, count, row) integer, intent(inout) :: row_cols(:,:) integer, intent(in) :: count diff --git a/src/solver/fem_solver_implicit.f90 b/src/solver/fem_solver_implicit.f90 index d8aff60..a10577b 100644 --- a/src/solver/fem_solver_implicit.f90 +++ b/src/solver/fem_solver_implicit.f90 @@ -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(:) diff --git a/tests/module/test_solver_implicit.f90 b/tests/module/test_solver_implicit.f90 index 78f9b46..b0dfd1d 100644 --- a/tests/module/test_solver_implicit.f90 +++ b/tests/module/test_solver_implicit.f90 @@ -12,6 +12,8 @@ program test_solver_implicit call check_prescribed_value() call check_robin_explicit_force() call check_robin_implicit_spring() + call check_mpc_explicit_force() + call check_mpc_implicit_penalty() contains @@ -124,6 +126,74 @@ contains if (ierr .ne. FEM_SUCCESS) call fail(47) end subroutine check_robin_implicit_spring + subroutine check_mpc_explicit_force() + integer :: ierr + integer :: dof2 + integer :: dof3 + + call setup_base_tet(0, 0, ierr) + if (ierr .ne. FEM_SUCCESS) call fail(50) + call database_allocate_mpc(1, 2, ierr) + if (ierr .ne. FEM_SUCCESS) call fail(51) + + mpc_start(1) = 1 + mpc_count(1) = 2 + mpc_rhs(1) = 0.05_rk + mpc_penalty(1) = 10.0_rk + mpc_node(1) = 2 + mpc_dof(1) = 1 + mpc_coeff(1) = ONE + mpc_node(2) = 3 + mpc_dof(2) = 1 + mpc_coeff(2) = -2.0_rk + + dof2 = dof_id(1,2) + dof3 = dof_id(1,3) + u(dof2) = 0.4_rk + u(dof3) = 0.1_rk + fext = ZERO + + call boundary_apply_mpc_forces(ierr) + if (ierr .ne. FEM_SUCCESS) call fail(52) + call check_close(fext(dof2), -1.5_rk, 53) + call check_close(fext(dof3), 3.0_rk, 54) + + call database_finalize(ierr) + if (ierr .ne. FEM_SUCCESS) call fail(55) + end subroutine check_mpc_explicit_force + + subroutine check_mpc_implicit_penalty() + integer :: ierr + integer :: iter + real(rk) :: res_norm + integer :: dof + + call setup_single_free_dof_model(ierr) + if (ierr .ne. FEM_SUCCESS) call fail(60) + call database_allocate_mpc(1, 1, ierr) + if (ierr .ne. FEM_SUCCESS) call fail(61) + + mpc_start(1) = 1 + mpc_count(1) = 1 + mpc_rhs(1) = 0.25_rk + mpc_penalty(1) = 6.0_rk + mpc_node(1) = 2 + mpc_dof(1) = 1 + mpc_coeff(1) = ONE + + call implicit_solve_linear_static(ZERO, 50, 1.0e-12_rk, iter, res_norm, ierr) + if (ierr .ne. FEM_SUCCESS) call fail(62) + if (res_norm .gt. TOL) call fail(63) + + dof = dof_id(1,2) + call check_close(u(dof), 0.125_rk, 64) + call check_close(u(dof_id(2,2)), ZERO, 65) + call check_close(u(dof_id(3,2)), ZERO, 66) + + call database_finalize(ierr) + if (ierr .ne. FEM_SUCCESS) call fail(67) + end subroutine check_mpc_implicit_penalty + subroutine setup_pull_model(ierr) integer, intent(out) :: ierr @@ -156,6 +226,34 @@ contains ierr = FEM_SUCCESS end subroutine setup_pull_model + subroutine setup_single_free_dof_model(ierr) + integer, intent(out) :: ierr + + call setup_base_tet(4, 0, ierr) + if (ierr .ne. FEM_SUCCESS) return + + bc_target(1) = '1' + bc_dof_start(1) = 1 + bc_dof_end(1) = 3 + bc_value(1) = ZERO + + bc_target(2) = '2' + bc_dof_start(2) = 2 + bc_dof_end(2) = 3 + bc_value(2) = ZERO + + bc_target(3) = '3' + bc_dof_start(3) = 1 + bc_dof_end(3) = 3 + bc_value(3) = ZERO + + bc_target(4) = '4' + bc_dof_start(4) = 1 + bc_dof_end(4) = 3 + bc_value(4) = ZERO + ierr = FEM_SUCCESS + end subroutine setup_single_free_dof_model + subroutine setup_prescribed_model(ierr) integer, intent(out) :: ierr