Files
FemLite/tests/module/test_particle_facet_coupling.f90
T
2026-06-26 00:48:57 +08:00

475 lines
18 KiB
Fortran

program test_particle_facet_coupling
use fem_precision
use fem_error
use fem_particle_state
use fem_particle_contact
use fem_particle_facet
use fem_particle_facet_coupling
use fem_particle_facet_step
implicit none
real(rk), parameter :: TOL = 1.0e-10_rk
call check_triangle_measure()
call check_quad_measure()
call check_contact_force()
call check_apply_and_scan()
call check_coupling_step()
call check_power_balance()
call check_error_paths()
contains
subroutine check_triangle_measure()
real(rk) :: coords(3,3)
real(rk) :: velocities(3,3)
real(rk) :: point(3)
real(rk) :: point_velocity(3)
real(rk) :: closest(3)
real(rk) :: weights(3)
real(rk) :: normal(3)
real(rk) :: facet_velocity(3)
real(rk) :: gap
real(rk) :: overlap
real(rk) :: normal_speed
real(rk) :: tangent_speed
real(rk) :: area
real(rk) :: distance2
real(rk) :: projected(3)
real(rk) :: signed_dist
real(rk) :: centroid(3)
integer :: ierr
coords(:,1) = (/ ZERO, ZERO, ZERO /)
coords(:,2) = (/ ONE, ZERO, ZERO /)
coords(:,3) = (/ ZERO, ONE, ZERO /)
velocities = ZERO
point = (/ 0.25_rk, 0.25_rk, 0.20_rk /)
point_velocity = (/ ONE, ZERO, -2.0_rk /)
call pfacet_normal_area(3, coords, normal, area, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(10)
call check_vec(normal, (/ ZERO, ZERO, ONE /), 11)
call check_close(area, 0.5_rk, 12)
call pfacet_closest_point(3, coords, point, closest, weights, distance2, &
ierr)
if (ierr .ne. FEM_SUCCESS) call fail(13)
call check_vec(closest, (/ 0.25_rk, 0.25_rk, ZERO /), 14)
call check_vec(weights, (/ 0.50_rk, 0.25_rk, 0.25_rk /), 15)
call check_close(distance2, 0.04_rk, 16)
call pfacet_measure(3, coords, velocities, point, point_velocity, 0.30_rk, &
gap, normal, overlap, weights, closest, &
facet_velocity, normal_speed, tangent_speed, area, &
ierr)
if (ierr .ne. FEM_SUCCESS) call fail(17)
call check_close(gap, -0.10_rk, 18)
call check_close(overlap, 0.10_rk, 19)
call check_vec(normal, (/ ZERO, ZERO, ONE /), 20)
call check_vec(facet_velocity, (/ ZERO, ZERO, ZERO /), 21)
call check_close(normal_speed, -2.0_rk, 22)
call check_close(tangent_speed, ONE, 23)
call pfacet_project_plane(3, coords, point, projected, signed_dist, normal, &
ierr)
if (ierr .ne. FEM_SUCCESS) call fail(24)
call check_vec(projected, (/ 0.25_rk, 0.25_rk, ZERO /), 25)
call check_close(signed_dist, 0.20_rk, 26)
call pfacet_centroid(3, coords, centroid, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(27)
call check_vec(centroid, (/ ONE / 3.0_rk, ONE / 3.0_rk, ZERO /), 28)
end subroutine check_triangle_measure
subroutine check_quad_measure()
real(rk) :: coords(3,4)
real(rk) :: velocities(3,4)
real(rk) :: point(3)
real(rk) :: closest(3)
real(rk) :: weights(4)
real(rk) :: normal(3)
real(rk) :: facet_velocity(3)
real(rk) :: node_forces(3,4)
real(rk) :: distance2
real(rk) :: gap
real(rk) :: overlap
real(rk) :: normal_speed
real(rk) :: tangent_speed
real(rk) :: area
integer :: ierr
coords(:,1) = (/ ZERO, ZERO, ZERO /)
coords(:,2) = (/ ONE, ZERO, ZERO /)
coords(:,3) = (/ ONE, ONE, ZERO /)
coords(:,4) = (/ ZERO, ONE, ZERO /)
velocities(:,1) = (/ ZERO, ZERO, ZERO /)
velocities(:,2) = (/ ONE, ZERO, ZERO /)
velocities(:,3) = (/ ONE, ONE, ZERO /)
velocities(:,4) = (/ ZERO, ONE, ZERO /)
point = (/ 0.75_rk, 0.75_rk, 0.25_rk /)
call pfacet_normal_area(4, coords, normal, area, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(40)
call check_vec(normal, (/ ZERO, ZERO, ONE /), 41)
call check_close(area, ONE, 42)
call pfacet_closest_point(4, coords, point, closest, weights, distance2, &
ierr)
if (ierr .ne. FEM_SUCCESS) call fail(43)
call check_vec(closest, (/ 0.75_rk, 0.75_rk, ZERO /), 44)
call check_close(sum(weights), ONE, 45)
call check_close(distance2, 0.0625_rk, 46)
call pfacet_interp_vector(4, velocities, weights, facet_velocity, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(47)
call check_vec(facet_velocity, (/ 0.75_rk, 0.75_rk, ZERO /), 48)
call pfacet_distribute_load(4, weights, (/ ZERO, ZERO, -8.0_rk /), &
node_forces, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(49)
call check_close(sum(node_forces(3,:)), -8.0_rk, 50)
call pfacet_measure(4, coords, velocities, point, (/ ZERO, ZERO, -ONE /), &
0.50_rk, gap, normal, overlap, weights, closest, &
facet_velocity, normal_speed, tangent_speed, area, &
ierr)
if (ierr .ne. FEM_SUCCESS) call fail(51)
call check_close(gap, -0.25_rk, 52)
call check_close(overlap, 0.25_rk, 53)
call check_close(normal_speed, -ONE, 54)
call check_close(tangent_speed, sqrt(1.125_rk), 55)
end subroutine check_quad_measure
subroutine check_contact_force()
type(particle_contact_params) :: params
real(rk) :: coords(3,3)
real(rk) :: velocities(3,3)
real(rk) :: force_particle(3)
real(rk) :: node_forces(3,3)
real(rk) :: weights(3)
real(rk) :: gap
real(rk) :: overlap
real(rk) :: normal(3)
real(rk) :: closest(3)
real(rk) :: normal_speed
real(rk) :: tangent_speed
real(rk) :: elastic_energy
real(rk) :: dissipated_power
real(rk) :: residual
integer :: slot
integer :: ierr
call setup_triangle(coords, velocities)
call particle_reset()
call particle_add(1, (/ 0.25_rk, 0.25_rk, 0.20_rk /), &
(/ ONE, ZERO, -ONE /), 0.30_rk, ONE, slot, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(70)
call particle_contact_init(1000.0_rk, 5.0_rk, 40.0_rk, 0.5_rk, &
1000.0_rk, 10.0_rk, params, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(71)
call pfacet_contact_force(slot, 3, coords, velocities, params, &
force_particle, node_forces, weights, gap, &
overlap, normal, closest, normal_speed, &
tangent_speed, elastic_energy, &
dissipated_power, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(72)
call check_close(overlap, 0.10_rk, 73)
call check_close(normal_speed, -ONE, 74)
call check_close(tangent_speed, ONE, 75)
call check_vec(force_particle, (/ -40.0_rk, ZERO, 110.0_rk /), 76)
call check_vec(weights, (/ 0.50_rk, 0.25_rk, 0.25_rk /), 77)
call check_close(sum(node_forces(1,:)), 40.0_rk, 78)
call check_close(sum(node_forces(3,:)), -110.0_rk, 79)
call check_close(elastic_energy, 5.0_rk, 80)
if (dissipated_power .le. ZERO) call fail(81)
call pfacet_force_balance(3, node_forces, force_particle, residual, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(82)
call check_close(residual, ZERO, 83)
end subroutine check_contact_force
subroutine check_apply_and_scan()
type(particle_contact_params) :: params
integer, parameter :: NF = 2
integer, parameter :: MAXN = 4
integer :: slot1
integer :: slot2
integer :: slots(2)
integer :: facet_nnode(NF)
integer :: contact_count
integer :: ierr
integer :: ext_id
logical :: active
real(rk) :: coords(3,MAXN,NF)
real(rk) :: velocities(3,MAXN,NF)
real(rk) :: node_forces(3,3)
real(rk) :: overlap
real(rk) :: max_overlap
real(rk) :: total_force(3)
real(rk) :: position(3)
real(rk) :: velocity(3)
real(rk) :: radius
real(rk) :: mass
real(rk) :: force(3)
coords = ZERO
velocities = ZERO
call setup_triangle(coords(:,1:3,1), velocities(:,1:3,1))
coords(:,1,2) = (/ ZERO, ZERO, 2.0_rk /)
coords(:,2,2) = (/ ONE, ZERO, 2.0_rk /)
coords(:,3,2) = (/ ZERO, ONE, 2.0_rk /)
facet_nnode = (/ 3, 3 /)
call particle_reset()
call particle_add(1, (/ 0.25_rk, 0.25_rk, 0.20_rk /), &
(/ ZERO, ZERO, -ONE /), 0.30_rk, ONE, slot1, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(100)
call particle_add(2, (/ 0.30_rk, 0.30_rk, 0.15_rk /), &
(/ ZERO, ZERO, -ONE /), 0.25_rk, ONE, slot2, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(101)
call particle_contact_init(1000.0_rk, ZERO, ZERO, ZERO, 800.0_rk, &
20.0_rk, params, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(102)
call pfacet_apply_contact(slot1, 3, coords(:,1:3,1), velocities(:,1:3,1), &
params, node_forces, overlap, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(103)
call check_close(overlap, 0.10_rk, 104)
call particle_get(slot1, ext_id, active, position, velocity, radius, mass, &
force, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(105)
call check_vec(force, (/ ZERO, ZERO, 100.0_rk /), 106)
call check_close(sum(node_forces(3,:)), -100.0_rk, 107)
call pfacet_scan_particle(slot1, NF, MAXN, facet_nnode, coords, velocities, &
params, contact_count, max_overlap, total_force, &
ierr)
if (ierr .ne. FEM_SUCCESS) call fail(108)
if (contact_count .ne. 1) call fail(109)
call check_close(max_overlap, 0.10_rk, 110)
call check_vec(total_force, (/ ZERO, ZERO, 100.0_rk /), 111)
slots = (/ slot1, slot2 /)
call pfacet_scan_cloud(2, slots, NF, MAXN, facet_nnode, coords, velocities, &
params, contact_count, max_overlap, total_force, &
ierr)
if (ierr .ne. FEM_SUCCESS) call fail(112)
if (contact_count .ne. 2) call fail(113)
call check_close(max_overlap, 0.10_rk, 114)
call check_vec(total_force, (/ ZERO, ZERO, 200.0_rk /), 115)
end subroutine check_apply_and_scan
subroutine check_coupling_step()
type(particle_contact_params) :: params
type(pfacet_step_stats) :: stats
integer, parameter :: NF = 2
integer, parameter :: MAXN = 4
integer :: slot1
integer :: slot2
integer :: slots(2)
integer :: facet_nnode(NF)
integer :: particle_count
integer :: contact_count
integer :: ierr
integer :: ext_id
logical :: active
real(rk) :: coords(3,MAXN,NF)
real(rk) :: velocities(3,MAXN,NF)
real(rk) :: node_loads(3,MAXN,NF)
real(rk) :: max_overlap
real(rk) :: total_energy
real(rk) :: total_power
real(rk) :: force_norm
real(rk) :: node_force_norm
real(rk) :: position(3)
real(rk) :: velocity(3)
real(rk) :: radius
real(rk) :: mass
real(rk) :: force(3)
coords = ZERO
velocities = ZERO
node_loads = ZERO
call setup_triangle(coords(:,1:3,1), velocities(:,1:3,1))
coords(:,1,2) = (/ ZERO, ZERO, 2.0_rk /)
coords(:,2,2) = (/ ONE, ZERO, 2.0_rk /)
coords(:,3,2) = (/ ZERO, ONE, 2.0_rk /)
facet_nnode = (/ 3, 3 /)
call particle_reset()
call particle_add(1, (/ 0.25_rk, 0.25_rk, 0.20_rk /), &
(/ ZERO, ZERO, ZERO /), 0.30_rk, ONE, slot1, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(120)
call particle_add(2, (/ 0.30_rk, 0.30_rk, 0.15_rk /), &
(/ ZERO, ZERO, ZERO /), 0.25_rk, ONE, slot2, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(121)
call particle_contact_init(1000.0_rk, ZERO, ZERO, ZERO, 1000.0_rk, ZERO, &
params, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(122)
call pfacet_step_reset(stats)
slots = (/ slot1, slot2 /)
call pfacet_step_cloud(2, slots, NF, MAXN, facet_nnode, coords, velocities, &
params, node_loads, stats, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(123)
call pfacet_step_summary(stats, particle_count, contact_count, max_overlap, &
total_energy, total_power, force_norm, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(124)
if (particle_count .ne. 2) call fail(125)
if (contact_count .ne. 2) call fail(126)
call check_close(max_overlap, 0.10_rk, 127)
call check_close(total_energy, 10.0_rk, 128)
call check_close(total_power, ZERO, 129)
call check_close(sum(node_loads(3,:,1)), -200.0_rk, 130)
call check_close(sum(node_loads(3,:,2)), ZERO, 131)
call pfacet_step_force_norms(stats, force_norm, node_force_norm, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(132)
call check_close(force_norm, 200.0_rk, 133)
call check_close(node_force_norm, 200.0_rk, 134)
call particle_get(slot1, ext_id, active, position, velocity, radius, mass, &
force, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(135)
call check_vec(force, (/ ZERO, ZERO, 100.0_rk /), 136)
call particle_get(slot2, ext_id, active, position, velocity, radius, mass, &
force, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(137)
call check_vec(force, (/ ZERO, ZERO, 100.0_rk /), 138)
call pfacet_step_validate(stats, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(139)
end subroutine check_coupling_step
subroutine check_power_balance()
type(particle_contact_params) :: params
real(rk) :: coords(3,3)
real(rk) :: velocities(3,3)
real(rk) :: force_particle(3)
real(rk) :: node_forces(3,3)
real(rk) :: weights(3)
real(rk) :: gap
real(rk) :: overlap
real(rk) :: normal(3)
real(rk) :: closest(3)
real(rk) :: normal_speed
real(rk) :: tangent_speed
real(rk) :: elastic_energy
real(rk) :: dissipated_power
real(rk) :: power
integer :: slot
integer :: ierr
call setup_triangle(coords, velocities)
velocities(:,1) = (/ 0.5_rk, ZERO, ZERO /)
velocities(:,2) = (/ 0.5_rk, ZERO, ZERO /)
velocities(:,3) = (/ 0.5_rk, ZERO, ZERO /)
call particle_reset()
call particle_add(1, (/ 0.25_rk, 0.25_rk, 0.20_rk /), &
(/ 0.5_rk, ZERO, -ONE /), 0.30_rk, ONE, slot, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(140)
call particle_contact_init(1000.0_rk, ZERO, 40.0_rk, 0.5_rk, 1000.0_rk, &
ZERO, params, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(141)
call pfacet_contact_force(slot, 3, coords, velocities, params, &
force_particle, node_forces, weights, gap, &
overlap, normal, closest, normal_speed, &
tangent_speed, elastic_energy, &
dissipated_power, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(142)
call check_close(tangent_speed, ZERO, 143)
call pfacet_contact_power(force_particle, (/ 0.5_rk, ZERO, -ONE /), 3, &
node_forces, velocities, power, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(144)
call check_close(power, -100.0_rk, 145)
end subroutine check_power_balance
subroutine check_error_paths()
type(particle_contact_params) :: params
integer :: slot
integer :: ierr
real(rk) :: bad_coords(3,3)
real(rk) :: coords(3,3)
real(rk) :: velocities(3,3)
real(rk) :: normal(3)
real(rk) :: area
real(rk) :: closest(3)
real(rk) :: weights(3)
real(rk) :: distance2
real(rk) :: node_forces(3,3)
real(rk) :: overlap
integer :: facet_nnode(1)
real(rk) :: scan_coords(3,3,1)
real(rk) :: scan_vel(3,3,1)
integer :: contact_count
real(rk) :: max_overlap
real(rk) :: total_force(3)
bad_coords = ZERO
call pfacet_normal_area(3, bad_coords, normal, area, ierr)
if (ierr .eq. FEM_SUCCESS) call fail(160)
call setup_triangle(coords, velocities)
call pfacet_closest_point(2, coords(:,1:2), (/ ZERO, ZERO, ZERO /), &
closest, weights(1:2), distance2, ierr)
if (ierr .eq. FEM_SUCCESS) call fail(161)
weights = (/ 0.5_rk, 0.6_rk, -0.1_rk /)
call pfacet_check_weights(3, weights, ierr)
if (ierr .eq. FEM_SUCCESS) call fail(162)
call particle_reset()
call particle_add(1, (/ 0.25_rk, 0.25_rk, 0.20_rk /), &
(/ ZERO, ZERO, -ONE /), 0.30_rk, ONE, slot, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(163)
call particle_contact_init(1000.0_rk, ZERO, ZERO, ZERO, 1000.0_rk, ZERO, &
params, ierr)
if (ierr .ne. FEM_SUCCESS) call fail(164)
call pfacet_apply_contact(slot, 2, coords(:,1:2), velocities(:,1:2), &
params, node_forces(:,1:2), overlap, ierr)
if (ierr .eq. FEM_SUCCESS) call fail(165)
facet_nnode(1) = 2
scan_coords(:,:,1) = coords
scan_vel(:,:,1) = velocities
call pfacet_scan_particle(slot, 1, 3, facet_nnode, scan_coords, scan_vel, &
params, contact_count, max_overlap, total_force, &
ierr)
if (ierr .eq. FEM_SUCCESS) call fail(166)
end subroutine check_error_paths
subroutine setup_triangle(coords, velocities)
real(rk), intent(out) :: coords(3,3)
real(rk), intent(out) :: velocities(3,3)
coords(:,1) = (/ ZERO, ZERO, ZERO /)
coords(:,2) = (/ ONE, ZERO, ZERO /)
coords(:,3) = (/ ZERO, ONE, ZERO /)
velocities = ZERO
end subroutine setup_triangle
subroutine check_close(actual, expected, code)
real(rk), intent(in) :: actual
real(rk), intent(in) :: expected
integer, intent(in) :: code
if (abs(actual - expected) .gt. TOL * max(ONE, abs(expected))) call fail(code)
end subroutine check_close
subroutine check_vec(actual, expected, code)
real(rk), intent(in) :: actual(:)
real(rk), intent(in) :: expected(:)
integer, intent(in) :: code
if (size(actual) .ne. size(expected)) call fail(code)
if (maxval(abs(actual - expected)) .gt. TOL) call fail(code)
end subroutine check_vec
subroutine fail(code)
integer, intent(in) :: code
write(*,*) 'test_particle_facet_coupling failed:', code
stop 1
end subroutine fail
end program test_particle_facet_coupling