program test_shell_finite_kinematics use fem_precision use fem_error use fem_database use fem_shell_rotation, only: shell_rotation_set_committed, & shell_rotation_rotate_vector use fem_shell_finite_kinematics implicit none real(rk), parameter :: TOL = 1.0e-10_rk real(rk), parameter :: SMALL_TOL = 1.0e-8_rk real(rk), parameter :: BMAT_TOL = 2.0e-6_rk real(rk), parameter :: HESS_TOL = 2.0e-4_rk call check_flat_membrane() call check_flat_bending() call check_mitc_constant_shear() call check_warped_initial_zero() call check_rigid_rotation_from_state() call check_spherical_cap_geom() call check_spherical_normal_offset() call check_bmat_fd_warped() call check_bmat_fd_spherical() contains subroutine check_flat_membrane() real(rk) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk) :: natural(NDIM) real(rk) :: strain_mem(3), strain_ben(3) real(rk) :: shear_direct(2), shear_mitc(2) real(rk) :: ref_cov(NDIM,2), cur_cov(NDIM,2) real(rk) :: ref_metric(3), cur_metric(3), detj real(rk) :: exx, eyy, gxy integer :: ierr, node exx = 2.0e-7_rk eyy = -3.0e-7_rk gxy = 4.0e-7_rk call flat_patch(ref_xyz, ref_dir) cur_xyz = ref_xyz cur_dir = ref_dir do node = 1, 4 cur_xyz(1,node) = cur_xyz(1,node) + exx * ref_xyz(1,node) + & HALF * gxy * ref_xyz(2,node) cur_xyz(2,node) = cur_xyz(2,node) + eyy * ref_xyz(2,node) + & HALF * gxy * ref_xyz(1,node) end do natural = ZERO call shell4_finite_kinematics_at(ref_xyz, cur_xyz, ref_dir, cur_dir, & natural, ref_cov, cur_cov, ref_metric, cur_metric, strain_mem, & strain_ben, shear_direct, shear_mitc, detj, ierr) if (ierr .ne. FEM_SUCCESS) call fail(10) call assert_close(strain_mem(1), exx, SMALL_TOL, 11) call assert_close(strain_mem(2), eyy, SMALL_TOL, 12) call assert_close(strain_mem(3), gxy, SMALL_TOL, 13) call assert_vector_zero(strain_ben, 3, SMALL_TOL, 14) call assert_vector_zero(shear_direct, 2, SMALL_TOL, 15) call assert_vector_zero(shear_mitc, 2, SMALL_TOL, 16) if (detj .le. ZERO) call fail(17) end subroutine check_flat_membrane subroutine check_flat_bending() real(rk) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk) :: natural(NDIM) real(rk) :: strain_mem(3), strain_ben(3) real(rk) :: shear_direct(2), shear_mitc(2) real(rk) :: ref_cov(NDIM,2), cur_cov(NDIM,2) real(rk) :: ref_metric(3), cur_metric(3), detj real(rk) :: kx, ky, kxy, theta_x, theta_y integer :: ierr, node kx = 3.0e-7_rk ky = -5.0e-7_rk kxy = 7.0e-7_rk call flat_patch(ref_xyz, ref_dir) cur_xyz = ref_xyz cur_dir = ref_dir do node = 1, 4 theta_y = kx * ref_xyz(1,node) + HALF * kxy * ref_xyz(2,node) theta_x = -ky * ref_xyz(2,node) - HALF * kxy * ref_xyz(1,node) cur_dir(:,node) = (/ theta_y, -theta_x, ONE /) call normalize(cur_dir(:,node)) end do natural = ZERO call shell4_finite_kinematics_at(ref_xyz, cur_xyz, ref_dir, cur_dir, & natural, ref_cov, cur_cov, ref_metric, cur_metric, strain_mem, & strain_ben, shear_direct, shear_mitc, detj, ierr) if (ierr .ne. FEM_SUCCESS) call fail(20) call assert_vector_zero(strain_mem, 3, SMALL_TOL, 21) call assert_close(strain_ben(1), kx, SMALL_TOL, 22) call assert_close(strain_ben(2), ky, SMALL_TOL, 23) call assert_close(strain_ben(3), kxy, SMALL_TOL, 24) end subroutine check_flat_bending subroutine check_mitc_constant_shear() real(rk) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk) :: natural(NDIM) real(rk) :: strain_mem(3), strain_ben(3) real(rk) :: shear_direct(2), shear_mitc(2) real(rk) :: ref_cov(NDIM,2), cur_cov(NDIM,2) real(rk) :: ref_metric(3), cur_metric(3), detj real(rk) :: gxz, gyz integer :: ierr, node gxz = 2.5e-6_rk gyz = -1.5e-6_rk call flat_patch(ref_xyz, ref_dir) cur_xyz = ref_xyz cur_dir = ref_dir do node = 1, 4 cur_xyz(3,node) = cur_xyz(3,node) + gxz * ref_xyz(1,node) + & gyz * ref_xyz(2,node) end do natural = (/ -0.30_rk, 0.40_rk, ZERO /) call shell4_finite_kinematics_at(ref_xyz, cur_xyz, ref_dir, cur_dir, & natural, ref_cov, cur_cov, ref_metric, cur_metric, strain_mem, & strain_ben, shear_direct, shear_mitc, detj, ierr) if (ierr .ne. FEM_SUCCESS) call fail(30) call assert_close(shear_direct(1), gxz, SMALL_TOL, 31) call assert_close(shear_direct(2), gyz, SMALL_TOL, 32) call assert_close(shear_mitc(1), shear_direct(1), SMALL_TOL, 33) call assert_close(shear_mitc(2), shear_direct(2), SMALL_TOL, 34) end subroutine check_mitc_constant_shear subroutine check_warped_initial_zero() real(rk) :: ref_xyz(NDIM,4), ref_dir(NDIM,4) call warped_patch(ref_xyz, ref_dir) call check_zero_state(ref_xyz, ref_xyz, ref_dir, ref_dir, 40) end subroutine check_warped_initial_zero subroutine check_rigid_rotation_from_state() real(rk) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk) :: quat(4), axis(NDIM), trans(NDIM) integer :: ierr, node call warped_patch(ref_xyz, ref_dir) axis = (/ 0.30_rk, -0.50_rk, 0.80_rk /) call normalize(axis) call quaternion_from_axis(axis, 1.35_rk, quat) trans = (/ 2.0_rk, -1.0_rk, 0.40_rk /) call database_allocate_model(4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ierr) if (ierr .ne. FEM_SUCCESS) call fail(50) do node = 1, 4 call shell_rotation_set_committed(node, quat, ierr) if (ierr .ne. FEM_SUCCESS) call fail(51) call shell_rotation_rotate_vector(node, ref_xyz(:,node), cur_xyz(:,node), & ierr, .false.) if (ierr .ne. FEM_SUCCESS) call fail(52) cur_xyz(:,node) = cur_xyz(:,node) + trans call shell_rotation_rotate_vector(node, ref_dir(:,node), cur_dir(:,node), & ierr, .false.) if (ierr .ne. FEM_SUCCESS) call fail(53) end do call check_zero_state(ref_xyz, cur_xyz, ref_dir, cur_dir, 60) call database_finalize(ierr) if (ierr .ne. FEM_SUCCESS) call fail(69) end subroutine check_rigid_rotation_from_state subroutine check_spherical_cap_geom() real(rk) :: ref_xyz(NDIM,4), ref_dir(NDIM,4) call spherical_cap_patch(ref_xyz, ref_dir) call check_zero_state(ref_xyz, ref_xyz, ref_dir, ref_dir, 70) end subroutine check_spherical_cap_geom subroutine check_spherical_normal_offset() real(rk) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk) :: eps_out(8), eps_in(8) real(rk) :: natural(NDIM) real(rk) :: offset real(rk) :: radius real(rk) :: expected_mem real(rk) :: expected_ben integer :: ierr integer :: node offset = 1.0e-3_rk radius = 10.0_rk expected_mem = offset / radius + HALF * (offset / radius)**2 expected_ben = offset / (radius * radius) natural = ZERO call spherical_cap_patch(ref_xyz, ref_dir) cur_dir = ref_dir cur_xyz = ref_xyz do node = 1, 4 cur_xyz(:,node) = ref_xyz(:,node) + offset * ref_dir(:,node) end do call generalized_strain(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, & eps_out, ierr) if (ierr .ne. FEM_SUCCESS) call fail(80) cur_xyz = ref_xyz do node = 1, 4 cur_xyz(:,node) = ref_xyz(:,node) - offset * ref_dir(:,node) end do call generalized_strain(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, & eps_in, ierr) if (ierr .ne. FEM_SUCCESS) call fail(81) if (min(eps_out(1), eps_out(2)) .le. ZERO) call fail(82) if (max(eps_in(1), eps_in(2)) .ge. ZERO) call fail(83) if (min(eps_out(4), eps_out(5)) .le. ZERO) call fail(84) if (max(eps_in(4), eps_in(5)) .ge. ZERO) call fail(85) call assert_close(HALF * (eps_out(1) + eps_out(2)), expected_mem, & 5.0e-6_rk, 88) call assert_close(HALF * (eps_out(4) + eps_out(5)), expected_ben, & 5.0e-7_rk, 89) if (maxval(abs(eps_out(7:8))) .gt. SMALL_TOL) call fail(86) if (maxval(abs(eps_in(7:8))) .gt. SMALL_TOL) call fail(87) end subroutine check_spherical_normal_offset subroutine check_bmat_fd_warped() real(rk) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk) :: natural(NDIM) real(rk) :: rotvec(NDIM) integer :: node call warped_patch(ref_xyz, ref_dir) cur_xyz = ref_xyz do node = 1, 4 cur_xyz(1,node) = cur_xyz(1,node) + & 0.12_rk * ref_xyz(1,node) - & 0.03_rk * ref_xyz(2,node) cur_xyz(2,node) = cur_xyz(2,node) - & 0.04_rk * ref_xyz(1,node) + & 0.09_rk * ref_xyz(2,node) cur_xyz(3,node) = cur_xyz(3,node) + 0.02_rk + & 0.05_rk * ref_xyz(1,node) * ref_xyz(2,node) rotvec = (/ 0.03_rk * real(node, rk), & -0.02_rk + 0.01_rk * real(node, rk), & 0.04_rk - 0.005_rk * real(node, rk) /) call rotate_by_rotvec(rotvec, ref_dir(:,node), cur_dir(:,node)) end do natural = (/ -0.23_rk, 0.37_rk, ZERO /) call assert_bmat_fd(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, 100) end subroutine check_bmat_fd_warped subroutine check_bmat_fd_spherical() real(rk) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk) :: natural(NDIM) real(rk) :: rotvec(NDIM) integer :: node call spherical_cap_patch(ref_xyz, ref_dir) cur_xyz = ref_xyz do node = 1, 4 cur_xyz(1,node) = cur_xyz(1,node) + 0.04_rk * real(node - 2, rk) cur_xyz(2,node) = cur_xyz(2,node) - 0.03_rk * real(node - 1, rk) cur_xyz(3,node) = cur_xyz(3,node) - 0.08_rk + & 0.01_rk * ref_xyz(1,node) rotvec = (/ -0.015_rk * real(node, rk), & 0.025_rk + 0.006_rk * real(node, rk), & 0.010_rk * real(3 - node, rk) /) call rotate_by_rotvec(rotvec, ref_dir(:,node), cur_dir(:,node)) end do natural = (/ 0.42_rk, -0.31_rk, ZERO /) call assert_bmat_fd(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, 200) end subroutine check_bmat_fd_spherical subroutine check_zero_state(ref_xyz, cur_xyz, ref_dir, cur_dir, code) real(rk), intent(in) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk), intent(in) :: ref_dir(NDIM,4), cur_dir(NDIM,4) integer, intent(in) :: code real(rk) :: natural(NDIM), strain_mem(3), strain_ben(3) real(rk) :: shear_direct(2), shear_mitc(2) real(rk) :: ref_cov(NDIM,2), cur_cov(NDIM,2) real(rk) :: ref_metric(3), cur_metric(3), detj real(rk) :: gps(2,4) integer :: ierr, q gps(:,1) = (/ -0.5773502691896258_rk, -0.5773502691896258_rk /) gps(:,2) = (/ 0.5773502691896258_rk, -0.5773502691896258_rk /) gps(:,3) = (/ 0.5773502691896258_rk, 0.5773502691896258_rk /) gps(:,4) = (/ -0.5773502691896258_rk, 0.5773502691896258_rk /) do q = 1, 4 natural = (/ gps(1,q), gps(2,q), ZERO /) call shell4_finite_kinematics_at(ref_xyz, cur_xyz, ref_dir, cur_dir, & natural, ref_cov, cur_cov, ref_metric, cur_metric, strain_mem, & strain_ben, shear_direct, shear_mitc, detj, ierr) if (ierr .ne. FEM_SUCCESS) call fail(code) call assert_vector_zero(strain_mem, 3, TOL, code + 1) call assert_vector_zero(strain_ben, 3, TOL, code + 2) call assert_vector_zero(shear_direct, 2, TOL, code + 3) call assert_vector_zero(shear_mitc, 2, TOL, code + 4) if (detj .le. ZERO) call fail(code + 5) end do end subroutine check_zero_state subroutine assert_bmat_fd(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, code) real(rk), intent(in) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk), intent(in) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk), intent(in) :: natural(NDIM) integer, intent(in) :: code real(rk) :: bmat(8,24) real(rk) :: bmat_ad(8,24) real(rk) :: hess_ad(8,24,24) real(rk) :: strain_ad(8) real(rk) :: strain0(8) real(rk) :: fd(8) real(rk) :: eps_plus(8) real(rk) :: eps_minus(8) real(rk) :: p_xyz(NDIM,4), m_xyz(NDIM,4) real(rk) :: p_dir(NDIM,4), m_dir(NDIM,4) real(rk) :: rotvec(NDIM) real(rk) :: detj real(rk) :: h real(rk) :: err real(rk) :: scale integer :: ierr integer :: col integer :: node integer :: local_dof h = 1.0e-6_rk call shell4_finite_bmat_at(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, & bmat, detj, ierr) if (ierr .ne. FEM_SUCCESS) call fail(code) if (detj .le. ZERO) call fail(code + 1) call shell4_finite_derivs_at(ref_xyz, cur_xyz, ref_dir, cur_dir, & natural, strain_ad, bmat_ad, hess_ad, & detj, ierr) if (ierr .ne. FEM_SUCCESS) call fail(code + 5) call generalized_strain(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, & strain0, ierr) if (ierr .ne. FEM_SUCCESS) call fail(code + 6) if (maxval(abs(strain_ad - strain0)) .gt. TOL) call fail(code + 7) if (maxval(abs(bmat_ad - bmat)) .gt. BMAT_TOL) then write(*,*) 'AD B-matrix mismatch: code=', code, & ' err=', maxval(abs(bmat_ad - bmat)) call fail(code + 8) end if do col = 1, 24 node = (col - 1) / 6 + 1 local_dof = mod(col - 1, 6) + 1 p_xyz = cur_xyz m_xyz = cur_xyz p_dir = cur_dir m_dir = cur_dir if (local_dof .le. NDIM) then p_xyz(local_dof,node) = p_xyz(local_dof,node) + h m_xyz(local_dof,node) = m_xyz(local_dof,node) - h else rotvec = ZERO rotvec(local_dof - NDIM) = h call rotate_by_rotvec(rotvec, cur_dir(:,node), p_dir(:,node)) rotvec(local_dof - NDIM) = -h call rotate_by_rotvec(rotvec, cur_dir(:,node), m_dir(:,node)) end if call generalized_strain(ref_xyz, p_xyz, ref_dir, p_dir, natural, & eps_plus, ierr) if (ierr .ne. FEM_SUCCESS) call fail(code + 2) call generalized_strain(ref_xyz, m_xyz, ref_dir, m_dir, natural, & eps_minus, ierr) if (ierr .ne. FEM_SUCCESS) call fail(code + 3) fd = (eps_plus - eps_minus) / (2.0_rk * h) err = maxval(abs(fd - bmat(:,col))) scale = ONE + max(maxval(abs(fd)), maxval(abs(bmat(:,col)))) if (err .gt. BMAT_TOL * scale) then write(*,*) 'B-matrix audit failed: code=', code, ' col=', col, & ' err=', err, ' scale=', scale write(*,*) 'fd=', fd write(*,*) 'analytic=', bmat(:,col) call fail(code + 4) end if end do call assert_hessian_fd(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, & hess_ad, code + 20) end subroutine assert_bmat_fd subroutine assert_hessian_fd(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, & hess_ad, code) real(rk), intent(in) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk), intent(in) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk), intent(in) :: natural(NDIM) real(rk), intent(in) :: hess_ad(8,24,24) integer, intent(in) :: code real(rk) :: bplus(8,24) real(rk) :: bminus(8,24) real(rk) :: hfd(8,24) real(rk) :: p_xyz(NDIM,4), m_xyz(NDIM,4) real(rk) :: p_dir(NDIM,4), m_dir(NDIM,4) real(rk) :: rotvec(NDIM) real(rk) :: detj real(rk) :: h real(rk) :: err real(rk) :: scale integer :: ierr integer :: col integer :: node integer :: local_dof h = 2.0e-6_rk do col = 1, 24 node = (col - 1) / 6 + 1 local_dof = mod(col - 1, 6) + 1 p_xyz = cur_xyz m_xyz = cur_xyz p_dir = cur_dir m_dir = cur_dir if (local_dof .le. NDIM) then p_xyz(local_dof,node) = p_xyz(local_dof,node) + h m_xyz(local_dof,node) = m_xyz(local_dof,node) - h else rotvec = ZERO rotvec(local_dof - NDIM) = h call rotate_by_rotvec(rotvec, cur_dir(:,node), p_dir(:,node)) rotvec(local_dof - NDIM) = -h call rotate_by_rotvec(rotvec, cur_dir(:,node), m_dir(:,node)) end if call shell4_finite_bmat_at(ref_xyz, p_xyz, ref_dir, p_dir, natural, & bplus, detj, ierr) if (ierr .ne. FEM_SUCCESS) call fail(code + 1) call shell4_finite_bmat_at(ref_xyz, m_xyz, ref_dir, m_dir, natural, & bminus, detj, ierr) if (ierr .ne. FEM_SUCCESS) call fail(code + 2) hfd = (bplus - bminus) / (2.0_rk * h) err = maxval(abs(hfd - hess_ad(:,:,col))) scale = ONE + max(maxval(abs(hfd)), maxval(abs(hess_ad(:,:,col)))) if (err .gt. HESS_TOL * scale) then write(*,*) 'Hessian audit failed: code=', code, ' col=', col, & ' err=', err, ' scale=', scale call fail(code + 3) end if end do end subroutine assert_hessian_fd subroutine generalized_strain(ref_xyz, cur_xyz, ref_dir, cur_dir, natural, & generalized, ierr) real(rk), intent(in) :: ref_xyz(NDIM,4), cur_xyz(NDIM,4) real(rk), intent(in) :: ref_dir(NDIM,4), cur_dir(NDIM,4) real(rk), intent(in) :: natural(NDIM) real(rk), intent(out) :: generalized(8) integer, intent(out) :: ierr real(rk) :: strain_mem(3), strain_ben(3) real(rk) :: shear_direct(2), shear_mitc(2) real(rk) :: ref_cov(NDIM,2), cur_cov(NDIM,2) real(rk) :: ref_metric(3), cur_metric(3), detj call shell4_finite_kinematics_at(ref_xyz, cur_xyz, ref_dir, cur_dir, & natural, ref_cov, cur_cov, ref_metric, cur_metric, strain_mem, & strain_ben, shear_direct, shear_mitc, detj, ierr) if (ierr .ne. FEM_SUCCESS) return generalized(1:3) = strain_mem generalized(4:6) = strain_ben generalized(7:8) = shear_mitc end subroutine generalized_strain subroutine flat_patch(ref_xyz, ref_dir) real(rk), intent(out) :: ref_xyz(NDIM,4) real(rk), intent(out) :: ref_dir(NDIM,4) integer :: node ref_xyz(:,1) = (/ ZERO, ZERO, ZERO /) ref_xyz(:,2) = (/ 2.0_rk, ZERO, ZERO /) ref_xyz(:,3) = (/ 2.0_rk, 1.5_rk, ZERO /) ref_xyz(:,4) = (/ ZERO, 1.5_rk, ZERO /) do node = 1, 4 ref_dir(:,node) = (/ ZERO, ZERO, ONE /) end do end subroutine flat_patch subroutine warped_patch(ref_xyz, ref_dir) real(rk), intent(out) :: ref_xyz(NDIM,4) real(rk), intent(out) :: ref_dir(NDIM,4) integer :: node ref_xyz(:,1) = (/ ZERO, ZERO, ZERO /) ref_xyz(:,2) = (/ 1.20_rk, 0.10_rk, 0.20_rk /) ref_xyz(:,3) = (/ 1.05_rk, 1.10_rk, 0.45_rk /) ref_xyz(:,4) = (/ -0.10_rk, 0.90_rk, 0.15_rk /) do node = 1, 4 ref_dir(:,node) = (/ 0.05_rk * real(node - 2, rk), & -0.03_rk * real(node - 1, rk), ONE /) call normalize(ref_dir(:,node)) end do end subroutine warped_patch subroutine spherical_cap_patch(ref_xyz, ref_dir) real(rk), intent(out) :: ref_xyz(NDIM,4) real(rk), intent(out) :: ref_dir(NDIM,4) real(rk) :: radius, x, y integer :: node radius = 10.0_rk ref_xyz(:,1) = cap_point(radius, -0.60_rk, -0.40_rk) ref_xyz(:,2) = cap_point(radius, 0.55_rk, -0.35_rk) ref_xyz(:,3) = cap_point(radius, 0.50_rk, 0.45_rk) ref_xyz(:,4) = cap_point(radius, -0.65_rk, 0.40_rk) do node = 1, 4 x = ref_xyz(1,node) y = ref_xyz(2,node) ref_dir(:,node) = (/ x, y, ref_xyz(3,node) - radius /) call normalize(ref_dir(:,node)) end do end subroutine spherical_cap_patch function cap_point(radius, x, y) result(point) real(rk), intent(in) :: radius, x, y real(rk) :: point(NDIM) point(1) = x point(2) = y point(3) = radius - sqrt(radius * radius - x * x - y * y) end function cap_point subroutine quaternion_from_axis(axis, angle, quat) real(rk), intent(in) :: axis(NDIM) real(rk), intent(in) :: angle real(rk), intent(out) :: quat(4) real(rk) :: half_angle half_angle = HALF * angle quat(1) = cos(half_angle) quat(2:4) = sin(half_angle) * axis end subroutine quaternion_from_axis subroutine normalize(vec) real(rk), intent(inout) :: vec(NDIM) real(rk) :: length length = sqrt(dot_product(vec, vec)) if (length .le. ZERO) call fail(900) vec = vec / length end subroutine normalize subroutine rotate_by_rotvec(rotvec, vec, rotated) real(rk), intent(in) :: rotvec(NDIM) real(rk), intent(in) :: vec(NDIM) real(rk), intent(out) :: rotated(NDIM) real(rk) :: axis(NDIM) real(rk) :: cross(NDIM) real(rk) :: angle real(rk) :: c real(rk) :: s angle = sqrt(dot_product(rotvec, rotvec)) if (angle .le. 1.0e-14_rk) then call cross_product(rotvec, vec, cross) rotated = vec + cross return end if axis = rotvec / angle call cross_product(axis, vec, cross) c = cos(angle) s = sin(angle) rotated = c * vec + s * cross + & (ONE - c) * dot_product(axis, vec) * axis end subroutine rotate_by_rotvec subroutine cross_product(a, b, c) real(rk), intent(in) :: a(NDIM) real(rk), intent(in) :: b(NDIM) real(rk), intent(out) :: c(NDIM) c(1) = a(2) * b(3) - a(3) * b(2) c(2) = a(3) * b(1) - a(1) * b(3) c(3) = a(1) * b(2) - a(2) * b(1) end subroutine cross_product subroutine assert_vector_zero(vec, n, tol, code) real(rk), intent(in) :: vec(:) integer, intent(in) :: n, code real(rk), intent(in) :: tol if (maxval(abs(vec(1:n))) .gt. tol) call fail(code) end subroutine assert_vector_zero subroutine assert_close(value, expected, tol, code) real(rk), intent(in) :: value, expected, tol integer, intent(in) :: code if (abs(value - expected) .gt. tol) call fail(code) end subroutine assert_close subroutine fail(code) integer, intent(in) :: code write(*,*) 'test_shell_finite_kinematics failed:', code stop 1 end subroutine fail end program test_shell_finite_kinematics