Add penalty MPC constraint support
This commit is contained in:
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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(:)
|
||||
|
||||
Reference in New Issue
Block a user