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